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ABSTRACT 


The  purpose  of  this  thesis  was  to  integrate  a  wall  following  motion  mode  for  a 
rigid  body  autonomous  vehicle.  Yamabico,  an  autonomous  vehicle  located  at  the  Naval 
Postgraduate  School,  was  used  as  the  test  and  evaluation  platform. 

To  implement  the  new  motion  mode,  the  vehicle  was  required  to  follow  a  straight 
wall  with  minor  variations,  navigate  around  comers,  and  avoid  obstacles  in  its  path  while 
maintaining  a  specified  offset  distance  from  continuously  connected  wall  segments. 
Sonar  transmitter/receiver  pairs  were  used  to  sense  the  environment  and  collect  positional 
data  for  analysis.  Modifications  to  pre-existing  motion  and  sensor  software  libraries  on 
board  Yamabico  were  performed  to  achieve  the  motion  goals.  One  of  the  major 
contributions  from  these  modifications  was  the  addition  of  a  linear  fitting  algorithm  using 
a  decay  factor.  The  algorithm  produced  quick  response  by  the  vehicle  to  changing 
conditions  in  its  environment. 

The  experimental  results  by  Yamabico  were  successful  with  the  algorithm 
developed  by  the  author.  The  result  of  this  thesis  is  that  an  autonomous  vehicle  can  be 
given  the  capability  to  perform  smooth  and  efficient  motion  adjustments  to  an 
environment  composed  of  orthogonal  wall  segments  and  obstacles. 
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I. 


INTRODUCTION 


A.  BACKGROUND 

All  biological  beings,  from  humans  to  insects,  share  the  ability  to  move  about  in 
their  world  without  constantly  bumping  into  obstacles  along  their  path  of  movement.  For 
insects  and  less  sophisticated  animals,  this  ability  is  simply  a  sense  and  react  type  of 
behavior.  More  sophisticated  species,  including  humans,  are  able  to  sense  their 
surroundings  with  multiple  sensors,  process  and  correlate  the  inputs,  and  perform 
complex  analyses  to  determine  future  movements. 

Robotics  and  artificial  intelligence  research  tries  to  emulate  these  behaviors.  On 
the  surface,  this  might  appear  to  be  a  relatively  straightforward  task.  In  reality,  even  the 
simplest  behaviors  can  encompass  an  enormous  amount  of  complexity,  and  emulating 
animal  behavior  in  an  artificial  system  is  extremely  difficult.  This  fact  is  evident  when 
considering  that  this  research  area  has  been  around  for  many  decades  while  yielding  few 
major  robotics  breakthroughs  in  natural  movement  abilities.  This  is  not  to  say  that  there 
has  been  no  significant  progress.  On  the  contrary,  there  have  been  many  important 
contributions  by  this  research  area  such  as  smart  weapons,  unmanned  aerial  vehicles,  and 
robots  sent  to  explore  solar  system  planets,  to  name  a  few.  These  examples  indicate  that 
robotics  and  artificial  intelligence  is  an  important  and  vital  area  of  research,  but  at  the 
same  time  they  show  that  implementing  even  simple  functional  behavior,  relative  to 
human  standards,  takes  years  to  research  and  develop  into  actual  working  models. 


1 


The  examples  mentioned  in  the  previous  paragraph  are  considered  to  have  simple 
functional  behavior  because  they  are  either  preprogrammed  to  perform  specific  tasks  or 
are  remotely  controlled  by  an  operator.  More  sophisticated  models  must  be  able  to 
perform  autonomously.  To  do  this,  an  artificial  entity  must  be  able  to  analyze  sensory 
inputs  and  make  decisions  based  on  that  analysis  without  outside  help.  This  process  is 
something  humans  do  easily  and,  for  the  most  part,  take  for  granted.  On  the  other  hand, 
implementing  this  process  in  hardware  and  software  requires  much  effort.  Dr.  Yutaka 
Kanayama  of  the  Naval  Postgraduate  School  has  been  working  on  this  problem  for  the 
better  part  of  twenty  years.  In  particular,  he  has  created,  maintained,  and  continuously 
modified  a  library  of  movement  and  sensory  algorithms  and  data  structures  called  the 
Model-based  Mobile  robot  Language  (MML).  The  robot  Yamabico  (Figure  1)  has  been 
the  testing  and  implementation  platform  for  experimentation  with  and  analysis  of  MML. 

B.  OVERVIEW 

This  thesis  is  an  extension  of  Dr.  Kanayama's  work  on  the  Yamabico  robot  and 
the  MML  software  library.  Yamabico  is  an  autonomous  robot  capable  of  using  its  sensor 
inputs  and  controlling  its  motion  based  on  those  inputs.  This  functionality  is  provided  by 
programming  code  written  in  the  C  programming  language  and  contained  in  the 
numerous  files  of  the  MML  software  library.  Yamabico  is  also  capable  of  following  a 
predefined  path  using  path  tracking  and  rotational  motion  modes. 
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Figure  1.  The  Autonomous  Vehicle  Yamabico  11. 
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In  addition  to  movement  capabilities,  MML  also  provides  the  facility  to  collect 
and  perform  analysis  on  sensor  data.  The  primary  sensor  for  Yamabico  is  sonar.  Sonar 
data  may  be  collected  by  one  or  more  of  the  twelve  sonars  arrayed  around  the  perimeter 
of  Yamabico.  Each  sonar,  when  enabled,  collects  data  including  the  distance  of  the  sonar 
return,  the  robot’s  position  and  orientation  at  the  time  of  sensing,  and  the  global  position 
of  the  sonar  return.  The  main  analysis  technique  offered  by  the  sonar  system  is  linear 
fitting  of  the  collected  data  points  using  the  least-squares  algorithm.  Each  sonar  is 
capable  of  maintaining  a  line  representation  of  its  data  collection.  This  ability  is 
important  when  used  for  representing  objects  in  the  robot's  world  and  maintaining  safe 
distances  or  defined  offsets  from  those  objects. 

The  tasks  Yamabico  performs  during  its  run  are  specified  in  a  user  file,  also 
written  in  C.  This  file  is  read  in  and  executed  following  initialization  of  the  robot.  It 
provides  the  interface  between  the  robot  and  the  outside  world.  In  this  file,  the 
programmer  may  provide  the  ability  to  set  initial  parameters  at  run  time,  specify  a  path  to 
follow,  and/or  stipulate  how  Yamabico  should  react  to  particular  input  events.  This  is 
accomplished  by  using  the  input  and  output  routines  of  C  and  by  calling  the  functions 
contained  in  MML. 

Yamabico  and  MML  provide  extensive  and  fairly  robust  motion  planning, 
sensing,  and  analysis  capabilities.  These  capabilities  can  be  used  to  direct  the  robot  to 
perform  many  different  tasks.  However,  there  is  currently  no  inherent  ability  for  the  robot 
to  follow  a  wall  or  avoid  obstacles  automatically.  These  tasks  could  be  programmed  into 
Yamabico  via  the  aforementioned  user  file,  but  this  would  be  inefficient.  Since  the  user 
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file  is  a  type  of  interface  between  the  outside  world  and  the  inner  workings  of  the  robot,  a 
level  of  abstraction  is  introduced.  This  means  more  overhead  in  the  program  and 
increased  timing  delays  between  the  software  and  the  hardware.  These  factors  can  cause 
significant  problems  in  the  robot’s  performance.  It  is  possible  to  overcome  these 
problems  by  careful  and  exhaustive  planning  and  programming,  but  it  would  be  much 
more  efficient  to  give  Yamabico  the  ability  to  perform  these  tasks  at  a  lower  level  (i.e.,  in 
the  MML  software.)  These  lower  level  functionalities  can  be  performed  by  an  additional 
motion  mode  contained  within  MML.  Implementing  this  new  motion  mode  in  the  MML 
software  is  the  focus  of  this  thesis. 

C.  PROBLEM  STATEMENT 

The  problem  we  want  to  solve  in  this  thesis  is  to  give  the  robot  the  capability  to 
make  a  complete  circuit  around  a  room  or  rooms  composed  of  continuously  connected 
orthogonal  wall  segments  while  avoiding  obstacles  in  its  path. 

To  solve  this  problem,  a  new  wall  following  motion  mode  needed  to  be  created. 

The  first  step  in  realizing  this  goal  was  to  get  the  robot  to  simply  follow  a  straight 
wall  while  maintaining  a  defined  offset.  The  wall  was  allowed  to  have  minor  variations 
in  its  "straightness",  requiring  the  robot  to  make  adjustments  to  its  path  as  it  proceeded 
along  the  wall. 

After  this  first  step  was  completed,  the  second  step  was  adding  the  ability  to 
negotiate  orthogonal  (90°)  turns.  To  attain  this  goal,  the  robot  had  to  handle  two  types  of 
turns:  (1)  turns  into  the  robot's  current  path  (concave  comers),  and  (2)  turns  away  from  its 
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path  (convex  comers).  Once  this  more  complex  functionality  was  implemented,  all  the 
tools  needed  by  the  robot  to  complete  a  circuit  around  a  room  were  in  place. 

The  capability  to  negotiate  obstacles  in  the  robot’s  path  was  a  side  effect  of 
solving  the  orthogonal  wall  following  problem.  Since  obstacles  in  the  motion  path  are  in 
close  proximity  to  the  wall  being  followed,  avoiding  them  was  a  natural  extension  of  the 
robot's  ability  to  turn  at  a  wall  intersection.  By  solving  the  orthogonal  turn  problem,  the 
requirements  to  avoid  obstacles  was  also  met. 

D.  ORGANIZATION 

Chapter  II  discusses  the  hardware  characteristics  of  Yamabico.  These  include  the 
motion  monitoring  and  control  system  and  the  sonar  control  system. 

Chapter  in  focuses  on  the  software  library.  The  approach  used  in  the  software  to 
model  physical  parameters  such  as  position  and  movement  is  somewhat  different  than 
what  is  considered  the  normal  representation.  An  explanation  of  the  primary 
characteristics  of  this  model  is  contained  in  the  first  part  of  the  chapter.  The  second  part 
of  the  chapter  discusses  the  components  of  MML  relevant  to  this  thesis  including  the 
currently  implemented  motion  modes  and  the  linear  fitting  algorithm. 

Chapter  IV  begins  the  discussion  on  thesis  specific  topics.  The  programming 
code  related  to  motion  modes  is  examined  in  more  detail  and  the  modifications  required 
to  implement  a  new  wall  following  mode  is  explained.  A  variation  of  the  linear  fitting 
algorithm  is  discussed  and  the  implication  of  using  such  a  variation  in  creating  this  new 
motion  mode  is  explained. 
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Chapter  V  talks  about  the  results  of  the  experiments  with  Yamabico  and  the 
conclusions  reached  after  the  successful  implementation  of  the  new  motion  mode  was 
completed. 
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II.  YAMABICO  HARDWARE 

All  specification  data  in  this  chapter  was  extracted  from  Reference  [1]  unless 
otherwise  noted. 

A.  GENERAL  OVERVIEW 

The  Yamabico  robot,  formally  Yamabico-1 1,  is  an  autonomous  robot  composed 
of  several  subsystems.  The  robot  stands  90  cm  tall  with  a  50  cm  by  50  cm  base.  The 
power  is  supplied  by  two  12-volt  rechargeable  batteries  that  drive  the  motors,  sonars,  and 
processor  boards.  Yamabico  can  also  be  connected  to  an  external  power  source  when 
stationary  to  conserve  battery  power. 

A  PowerBook™  145  laptop  computer  is  mounted  on  top  of  the  robot  and  is  used 
as  a  mediator  between  the  outside  user,  the  user  program  and  MML  software,  and  the 
hardware.  All  code  development  and  compilation  is  done  on  a  Sun  SPARCstation  10 
workstation  using  the  UNIX  operating  system. 

Once  a  program  is  compiled,  the  executable  is  downloaded  through  an  ethemet 
connection  to  a  SPARC  IV  main  CPU  board  on  a  VME  bus.  The  laptop  then  runs  the 
program  which  initializes  Yamabico's  hardware,  executes  the  user  program^  and  handles 
the  hardware  interrupts.  After  Yamabico  has  completed  its  run,  any  data  earmarked  for 
collection  via  MML  logging  functions  can  be  downloaded  from  the  laptop  to  the  UNIX 
network  through  a  standard  phone  line  cable. 

There  are  four  major  subsystems  in  Yamabico  that  interact  asynchronously  to  give 
it  its  capabilities.  The  motion  and  sonar  subsystems  are  the  first  two  and  are  the  focus  of 
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the  remainder  of  this  chapter.  The  software  library  and  user  program  comprise  the  third 
subsystem.  They  are  discussed  in  Chapter  HI.  Finally,  Yamabico  has  an  optical  image 
processing  subsystem,  but  this  subsystem  was  not  used  for  this  thesis  and  will  not  be 
discussed. 

B.  MOTION  MONITORING  AND  CONTROL  SYSTEM 

Yamabico  is  a  wheeled  differential  drive  vehicle.  Two  separate  35  Watt  DC 

motors  drive  each  wheel  via  drive  shafts  using  a  1 :24  gear  box  ratio.  The  drive  shafts  are 
mounted  with  optical  encoders  to  enable  the  robot  to  calculate  how  far  each  wheel  has 
traveled  in  a  given  time  step.  This  information  is  used  to  make  odometry  calculations. 
Four  shock  absorbing  caster  wheels  are  also  mounted  on  the  base  of  Yamabico  to  provide 
stability.  Since  this  is  an  indoor  vehicle,  there  was  less  emphasis  on  detailing  physical 
factors  such  as  slippage  and  uneven  terrain  effects  and  the  more  general  approach  of  dead 
reckoning  (DR)  was  used  in  movement  calculations. 

The  DR  approach  does  cause  navigational  problems  over  long  running  times.  For 
instance,  if  the  floor  is  extremely  slippery  and  a  high  acceleration  is  used,  the  wheels  may 
slip,  resulting  in  a  measured  travel  distance  that  is  larger  than  the  actual  true  distance  of 
travel.  As  another  example,  if  the  robot  drives  over  a  board  or  an  uneven  floor  segment, 
the  robot  may  measure  a  turn  when  no  turn  was  made.  These  errors  are  minor  and 
acceptable  over  small  distances.  When  the  robot  moves  over  much  larger  distances,  the 
errors  compound  until  eventually  they  are  no  longer  negligible.  At  this  point  adjustments 
have  to  be  made  by  the  robot  to  realign  itself  with  the  real  world.  Several  different 
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techniques  can  be  used  to  update  the  robot's  navigation.  In  a  known  or  partially  known 
world,  a  landmark  can  be  used  to  correct  navigational  errors.  Another  technique,  and  the 
one  used  in  this  thesis,  is  to  navigate  by  continuously  updating  its  position  and  future  path 
in  reference  to  the  world  (i.e.  follow  a  wall  or  walls.)  This  is  explored  in  detail  in 
Chapter  IV. 


The  motion  monitoring  system  interrupts  the  software  system  on  regular  1 0  ms 
intervals.  Yamabico's  nominal  speed  is  30  cm/sec  with  a  maximum  speed  of  65  cm/sec. 
At  the  nominal  speed,  interrupts  occur  every  0.3  cm  as  indicated  by  Equation  (2.1). 


30— xlO'2 
sec 


860  =03-  “* 


interrupt  interrupt 


(2.1) 


This  short  travel  distance  between  motion  updates  provides  a  sufficiently  high 
degree  of  accuracy  in  odometry  calculations.  During  each  interrupt,  information  from  the 
drive  shaft  encoders  is  fed  to  a  software  interrupt  routine.  The  software  can  then  use  this 
information  to  calculate  its  new  position,  make  adjustments  to  future  movement 
requirements,  and  return  commanded  linear  and  rotational  velocities  to  the  motion 
processor  board.  The  board  then  translates  these  commanded  velocities  into  electronic 
signals  to  the  two  wheel  motors  in  a  pulse-width  modulation  mode  to  effect  the  requested 
movement. 


C.  SONAR  AND  SONAR  CONTROL  SYSTEM 

Yamabico  s  sonar  system  consists  of  twelve  sonars  arranged  in  a  horizontal  plane 
approximately  30  cm  above  ground  level  and  angularly  spaced  in  30°  increments  to  give 
full  360°  coverage  (see  Figure  2).  Each  sonar  is  a  transmitter/receiver  pair  operating  at  a 
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frequency  of  40  kHz  and  may  be  individually  enabled  or  disabled  at  any  time  within  the 
user  program. 


Top  View 

i 

Forward 

r  ^ 

k 

4r 

A 

r 

* 

*  t  K 

Figure  2.  Sonar  Configuration  of  Yamabico  After  Ref.  [1] . 

The  sonars  are  controlled  by  a  separate  processing  board  on  Yamabico.  In  order 
to  reduce  sampling  time,  the  sonars  are  organized  into  3  groups  of  4  sonars  apiece.  Each 
group  contains  sonars  that  are  separated  by  90°  to  minimize  interference  from 
neighboring  sonar  transmissions.  All  enabled  sonars  within  the  same  group  are  pulsed 
simultaneously  thereby  reducing  the  time  spent  sampling  by  a  factor  of  four  when  all 
twelve  sonars  are  in  use.  The  sonar  groupings  are  shown  in  Table  1 . 
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Group 

Sonars  in  Group 

0 

0,2, 5,7 

1 

1,3, 4,  6 

2 

8,9, 10, 11 

Table  1.  Sonar  Groupings. 


As  in  the  motion  control  hardware,  the  sonar  system  generates  sonar  interrupt 
events  in  the  software.  These  interrupts  occur  at  50  ms  intervals.  Because  the  sonars  are 
activated  in  groups,  the  time  between  pings  of  a  single  sonar  can  take  up  to  150  ms. 
When  one  or  more  sonars  in  a  group  are  enabled,  that  group  generates  an  interrupt  and 
each  enabled  sonar  in  the  group  is  activated.  Similarly,  if  no  sonars  in  a  group  are 
enabled,  that  group  does  not  generate  an  interrupt.  With  these  facts  in  minH  the  time 
between  pings  on  an  single  enabled  sonar  has  a  minimum  value  of  50  ms  and  a  maximum 
value  of  150  ms.  This  design  is  much  more  efficient  than  activating  sonars  individually, 
which  could  result  in  a  maximum  of 600  ms  between  pings. 

In  the  original  design  of  the  sonar  system,  a  pulse  width  of  1  ms  was  used.  This 
was  later  changed  to  the  current  value  of  0.5  ms  to  reduce  the  minimum  range  of  the 
sonars.  Assuming  the  speed  of  sound  in  air  at  room  temperature  is  approximately  340 
m/s,  the  minimum  range  can  be  calculated  as  half  the  distance  the  sound  will  travel  in  the 
time  of  a  transmit  pulse  (pulse  width). 

minrange=H  34000— x  0.5  ms)  =8.5  cm  (22) 

2\  sec  J  v  '  J 
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The  system  actually  sets  the  minimum  range  to  be  9.6  cm  to  allow  for  circuit 
switching  and  settling  times. 

The  maximum  range  of  the  sonars  is  imposed  by  hardware  limitations.  When  a 
pulse  is  sent  out,  a  16  bit  data  register  is  used  to  count  time  until  the  pulse  is  received. 
Only  the  first  12  bits  of  this  register  are  used  for  counting,  limiting  the  counter  to  a 
maximum  value  of 4095  (212- 1).  The  counter  is  incremented  every  6  ps  which 
corresponds  to  an  incremental  distance  of  0.204  cm.  The  maximum  range  of  the  sonar  is 
thus: 

max  range = — (4095  x  0.204  cm) = 41 7.7  cm  (2.3) 

Again  the  system  allows  for  circuitry  timing  and  various  other  long  range  errors 
and  sets  the  maximum  range  to  be  409  cm. 

The  location  of  each  sonar  was  depicted  graphically  in  Figure  2.  In  order 
calculate  global  positions  of  the  sonar  returns,  the  software  system  maintains  a  table, 
similar  to  Table  2,  of  sonar  positions  relative  to  the  center  of  the  robot  with  the  x-axis 
aligned  to  the  front  of  the  robot  and  angles  measured  counter-clockwise  from  the  x-axis. 
Global  positions  are  then  calculated  using  the  robot's  current  position,  the  relative 
position  of  the  sonar  making  the  measurement,  and  the  distance  returned  by  the  sonar. 
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|  Sonar 

X  (cm) 

Y  (cm) 

6  (rad) 

23.6 

-0.5 

r  o 

i 

-23.0 

13.1 

5tt/6 

2 

-22.6 

- 1.0 

71 

3 

24.7 

-14.6 

-7t/6 

4 

13.4 

21.3 

7r/3 

5 

0.0 

20.6 

tc/2 

6 

-12.6 

-21.3 

-2tt/3 

7 

0.0 

-20.5 

-71/2 

8 

-13.4 

21.3 

2tt/3 

9 

-23.5 

-14.9 

-5tt/6 

10 

12.1 

-21.3 

-7t/3 

11 

25.2 

14.1 

7t/6 

Table  2 


Sonar  Positions  Relative  to  Center  of  Robot. 


III.  SOFTWARE  LIBRARY 

The  software  system  used  to  control  Yamabico  is  both  complex  and  robust. 
Interrupts  are  used  to  optimize  operations.  Timing  considerations  of  these  interrupts  are 
an  important  part  of  code  development.  The  MML  software  library  is  composed  of  many 
files  each  containing  software  functions  written  in  the  C  programming  language. 

Multiple  files  are  used  to  separate  and  organize  the  main  objectives  of  each  part  of  the 
system.  These  objectives  can  be  roughly  categorized  as  low  level  motion  control,  sonar 
control,  and  sonar  analysis,  and  high  level  geometry,  input/output,  and  user  control. 

A.  INTERRUPT  CONTROL  SYSTEM 

As  mentioned  in  Chapter  n,  Yamabico  operates  in  an  interrupt  based 
environment.  The  software  library  serves  as  a  multi-tasking  operating  system  for  the 
robot  [1].  It  manages  interrupt  requests  based  on  the  relative  importance  of  the  task 
associated  with  each  type  of  interrupt.  When  received,  higher  priority  interrupts  are 
handled  immediately  and  lower  priority  tasks  are  suspended  until  completion  of  the 
corresponding  interrupt  handling  routine. 

The  software  system  designates  eight  levels  (0  -  7)  of  interrupt  priorities.  Table  3 
depicts  these  levels  with  higher  level  numbers  indicating  higher  priority.  The  interrupt 
type  indicates  how  each  interrupt  level  interacts  with  the  rest  of  the  system. 

Asynchronous  interrupt  events  are  processed  immediately  while  synchronous  events,  as 
the  name  implies,  are  synchronized  with  circuitry  timing 
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Interrupt  Level 

Interrupt  Source 

Function 

Interrupt  Type 

7 

stop  button 

reset 

asynchronous 

6 

— 

not  used 

5 

— 

not  used 

— 

4 

Serial  Board  1 

locomotion 

synchronous 

3 

Serial  Board  2 

teletype 

asynchronous 

2 

Sonar  Board 

sonar 

synchronous 

1 

Serial  Board  0 

debugger 

synchronous 

! 

0 

user  instructions 

none 

Table  3.  Interrupt  Priority  Table  for  Yamabico  After  Ref. 
[1]  • 


A  reset  button  is  installed  on  top  of  Yamabico  where  it  is  readily  available  to  the 
operator.  When  depressed,  this  button  generates  an  interrupt  of  the  highest  level  (level  7). 
It  causes  the  robot  to  halt  immediately  and  resets  all  hardware  components.  This  button 
is  used  extensively  during  development  of  new  routines  for  Yamabico  because  invariably 
these  new  routines  have  flaws  in  logic  or  unexpected  side  effects.  By  using  the  reset 
button,  the  operator  may  cease  Yamabico's  movement  before  a  collision  occurs. 

Interrupt  levels  5  and  6  are  currently  unused. 

The  motion  system  is  incorporated  at  interrupt  level  4.  This  is  the  highest  priority 
interrupt  that  occurs  during  normal  operation  of  Yamabico.  Since  motion  hardware 
interrupts  occur  every  10  ms,  this  priority  level  reflects  the  importance  of  handling  this 
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type  of  event  immediately.  All  steering  and  odometry  functions  are  run  during  the 
processing  of  this  type  interrupt. 

All  console  operations  of  the  laptop  and  data  transfer  between  the  laptop  and  the 
UNIX  network  have  interrupt  priority  level  3.  This  allows  for  feedback  to  the  outside 
user,  input  from  the  user,  code  uploading,  and  data  downloading. 

The  sonar  system  operates  at  interrupt  level  2  allowing  sensor  updating  to  occur  at 
a  relatively  high  priority.  If  no  sonars  are  enabled,  this  level  is  bypassed  during  the 
interrupt  polling  procedure. 

At  the  lowest  levels  are  debugging  routines  at  level  1  and  the  user  control 
instructions  at  level  0.  All  other  interrupts  can  override  this  last  level.  In  general,  level  0 
can  be  thought  of  simply  as  code  that  runs  whenever  the  system  has  time  to  run  it.  This 
last  statement  may  seem  misleading  because  the  robot  won't  really  do  anything  without 
the  user  code.  It  is  in  this  part  of  the  software  that  the  initial  state  of  the  robot  is  set  (i.e., 
position,  initial  speed,  path,  enabled  sonars,  data  logging,  etc.)  and  end  of  run  conditions 
are  checked.  However,  since  these  functions  can  be  considered  as  initialization  routines 
(before  motion  and  sensing  starts)  or  short  status  checks  (during  run),  there  are  no  adverse 
effects  of  placing  them  at  the  lowest  interrupt  level. 

B.  TRANSFORMATION  MODEL  OF  MOTION 

Before  delving  into  the  details  of  the  motion  algorithms  in  MML,  an 
understanding  of  the  underlying  mathematical  model  must  be  gained.  In  most  physical 
applications,  Cartesian  or  polar  coordinates  are  use  to  model  kinematic  effects.  Dr. 
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Kanayama  has  developed  a  related  but  somewhat  different  and  more  versatile  model  of 
motion  of  a  rigid  body  robot.  This  model  is  based  on  the  fact  that  the  trajectory  of  a  rigid 
body  object  can  be  represented  by  a  sequence  of  points,  and  since  we  calculate  the 
vehicle's  position  at  discrete  intervals,  the  positional  changes  can  be  represented  by  a 
series  of  discrete  transformations.  These  transformations  are  the  heart  of  the  motion 
model.  This  section  presents  a  synopsis  of  Dr.  Kanayama's  transformation  model  of 
motion  [2, 3, 4,  5, 6]. 

1.  Transformations  and  Configurations 

A  transformation  is  described  by  three  parameters,  the  conventional  two- 
dimensional  Cartesian  coordinates  (x,  y)  and  an  orientation  angle  (0).  In  a  rigid  body 
vehicle,  these  correspond  to  a  translation  and  a  rotation  (or  heading  change).  Note  that  in 
this  model,  positive  angles  are  measured  in  the  counter-clockwise  direction  relative  to  a 
reference  (local  or  global)  x-axis.  There  is  also  the  idea  of  a  configuration.  A 
configuration  describes  the  current  state  of  the  vehicle  using  the  same  parameters  of  a 
transformation  but  with  somewhat  different  meanings.  In  a  configuration,  x  and  y 
represent  the  global  coordinates  of  a  local  reference  frame  and  0  represents  the  current 
orientation  of  the  local  x-axis.  Both  transformations  and  configurations  are  specified  by  a 
triple  (x,  y,  0).  The  main  difference  between  a  transformation  and  a  configuration  is  that 
the  former  acts  as  an  operator  and  the  latter  acts  as  a  spatial  specification  of  an  object. 
Figure  3  shows  an  example  of  an  initial  configuration  (q0)  which  undergoes  a 
transformation  (TO  resulting  in  a  new  configuration  (qi). 
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Figure  3.  Example  of  Transformation  of  a  Configuration . 


2.  Composition 

In  Figure  3,  to  get  from  q0  to  qh  the  transformation  Ti  is  applied  to  the 

configuration  qo  by  a  method  called  composition  represented  by  the  symbol  (°). 

Composition  is  defined  as  follows: 

l&t  q  o  {xq  ,y  q,Q  and  T\ 

Then : 
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(3.1) 


rx0  +x,  cos#0  -yx  sin0o 
^0+xisin^o  +3'i«>s0o 

^  0O  +0j 

In  this  thesis,  the  two  forms  seen  above  (i.e.  parameter  triple  and  column  vector) 
will  be  used  interchangeably  to  represent  a  transformation/configuration. 

3.  Inverse  and  Relative  Transformation 

In  Figure  3,  qx  =  q0  °  7] .  In  order  to  get  from  qi  back  to  qo,  an  inverse  operation 

is  needed  such  that  qx  ©  7J-1  =  #0  °  7]  ©  7]'1  =  #0 ,  where  7J'1  is  the  inverse  transformation 

of  7] .  When  a  transformation  is  composed  with  its  inverse,  it  produces  an  identity 

transformation  which  neutralizes  its  effect  in  an  expression  such  as  that  above.  The 

definition  of  an  inverse  transformation  is: 

LetT  =  (x,y,0), 

Them. 

-xcosO-ysinO' 

xsinO-ycosO  (3.2) 

-e 

One  use  for  the  inverse  function  is  in  finding  relative  transformations.  Suppose  a 
robot  vehicle  is  represented  by  the  global  configuration  qv,  and  an  object  is  represented 
by  the  global  configuration  qo,  as  shown  in  Figure  4.  To  find  the  object's  relative 
transformation  from  the  vehicle,  there  must  be  a  transformation  q*  such  that  qv  ©  q*  =  qo* 
Composing  both  sides  with  the  inverse  of  qv  results  in  q*  =  (qv)'1  ©  qo,  the 
transformation  of  the  object  relative  to  the  vehicle's  coordinate  frame. 
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Figure  4.  Example  of  a  Relative  Transformation  (q*) . 

4.  Lines,  Circles,  and  Paths 

For  path  planning,  the  transformation/configuration  model  is  expanded  so  that 
lines  and  circular  arcs  can  also  be  expressed  as  configurations.  For  this  purpose,  a 
configuration  is  redefined  by  adding  a  fourth  parameter.  This  fourth  parameter  (k) 
represents  the  curvature  of  the  configuration  where  1/k  is  the  radius  of  curvature.  Lines 
become  directed  lines  with  the  x  and  y  parameters  specifying  its  origin,  0  its  orientation, 
and  since  the  radius  of  curvature  of  a  line  is  infinite,  k  =  0.  For  example,  a  line  (L) 
beginning  at  the  global  position  (x  =  1,  y  =  5)  and  angled  45°  above  the  x-axis  would  be 
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defined  by  the  configuration  L  —  ^1,5,-j-  ,0J .  Circles  are  somewhat  more  complex,  but 

follow  similar  reasoning.  A  circle  configuration  is  a  directed  circle  starting  at  the  (x,  y) 
position  (on  the  circle),  oriented  in  the  0  direction  (tangent  to  the  circle),  with  curvature 
k  —  1 /radius.  For  example,  one  representation  of  a  counter-clockwise  circle  centered 


Figure  5.  Example  Line(L)  and  Circle (C)  Configurations 
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These  ideas  of  transformations  and  configurations  are  a  very  powerful  and  can  be 
applied  in  a  variety  of  ways.  For  instance,  to  specify  a  simple  square  pattern  starting  at 
the  origin,  only  one  configuration  and  one  transformation  is  needed.  The  following 
algorithm  will  produce  four  lines  making  the  square  shown  in  Figure  6: 


Figure  6.  Square  path  pattern  defined  by  an  initial 
configuration  and  successively  applying  a  transformation. 

Path  planning  for  Yamabico  is  accomplished  in  this  manner,  where  a  path  is 
specified  as  a  sequence  of  lines  and  circular  arcs. 
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5.  Circular  Transformation 

So  far  only  large  scale  transformations  have  been  depicted.  Yamabico  updates  its 
odometry  every  1 0  ms.  In  this  small  time  period,  the  distance  it  moves  is  very  small 
(~0.03  cm).  Taking  advantage  of  this  fact  another  concept,  called  a  circular 
transformation,  is  used  in  path  trajectory  calculation. 

First,  consider  a  path  (P)  which  can  be  defined  by  an  initial  configuration 
0o  =  (*o  >*o)  311(1  future  configurations  as  a  function  of  arc  length  s.  The  path 


is  completely  specified  by  k  as  a  function  of  s.  Because  the  curvature  is  the  derivative  of 


the  tangential  orientation 


along  P,  once  k(s)  is  defined,  0(s)  is  found  by: 


s 

0(s)  =  0o  +  jrc(u)du  (3.3) 

0 

Positions  along  P  are  also  functions  of  s  and  are  specified  by  the  following 
parametric  equations: 


s 

x(s)  =  x0  +  J  cos(  O^ujjdu 

0 

(3.4) 

S 

y(s)  =  y0  +  \sm[0{u))du 

0 

(3.5) 

These  equations  are  a  continuous  representation  of  a  path,  and  the  instantaneous 

configuration  of  a  vehicle  following  this  path  is: 

tf(5) = (*(■*)>  0(4  *(■*)) 

(3.6) 
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In  digital  computer  based  systems,  continuous  functions  are  represented  by  a 
series  of  discrete  changes  or  deltas  (A).  In  Yamabico,  a  continuous  path  is  represented  by 
a  series  of  discrete  instantaneous  configurations 

(3.7) 

Each  successive  configuration  can  be  found  by  the  following  iterative  technique: 

9m  =q,  °Aqi  (3.8) 

where  the  Aqi's  are  incremental  transformations  over  a  small  path  distance  As. 

If  As  is  sufficiently  small,  we  can  use  the  following  approximation  to  the  amount 
of  change  in  orientation  over  As: 

A0  =  x-(j)As  (3.9) 

To  improve  this  approximation  we  use  die  value  of  the  curvature  at  the  midpoint 
of  the  interval  [s,  s  +  As]: 

A0  =  *^+y)Aj  (3.10) 


If  it  is  also  assumed  that  the  curvature  along  the  interval  [s,  s  +  As]  remains 
constant,  then  the  interval  represents  a  circular  arc  of  radius  1/k.  Figure  7  depicts  this 
circular  arc  with  curve  length  As  and  orientation  change  A0  starting  at  the  origin  of  a 
reference  frame  with  initial  orientation  aligned  with  the  x-axis. 

The  radius  of  the  circular  arc  is: 


1  As 
r~rc~A0 


(3.11) 
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Figure  7.  Circular  Arc  Parameters  After  Ref.  [2]. 

An  expression  for  Aqj  can  now  be  formulated  as  a  circular  transformation,  denoted 
by  C(As,  A0).  First,  for  A0  *  0: 

C(As,  A 6?)  &  (Ax,  Ay,  A#) 

=  (rsinA0,r(l-cosA0),A#)  (3.12) 

sinA0V  f  l-cosA0 
A0  )  \  A0  . 


For  A0  =  0,  the  circular  arc  becomes  a  straight  line  and: 

C(As,A0)  =  (As,O,O)  (3.13) 

Finally,  |A0|  is  normally  small  enough  to  justify  using  a  Taylor  expansion  for  the 
sine  and  cosine  terms  in  Equation  (3.12).  With  these  expansions.  Equations  (3.12)  and 
(3.13)  can  be  condensed  into  a  single  equation: 
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C(As,A0)  = 


(f 


A02  A04 


3! 


5! 


As, - 

7  • 


1  A02  A04  \ 


2!  4! 


6! 


A0As,A0 


(3-14) 


Using  Equation  (3.14)  any  path  can  be  generated  using  the  following  algorithm: 


This  algorithm  closely  matches  the  algorithm  Yamabico  uses  to  calculate  its 
trajectory  while  it  is  in  motion.  The  robot  must  perform  some  extra  steps  such  as 
calculating  As  by  averaging  the  amount  of  rotation  of  each  wheel  and  determining  what 
kind  of  path  shape  it  is  following,  but  the  algorithm  presented  above  is  central  to 
computing  the  vehicle's  current  position. 

6.  Tracking  Lines 

In  the  preceding  discussions  a  method  for  specifying  a  desired  path  for  the  robot 
(lines  and  circles)  and  a  method  for  calculating  the  robot's  trajectory  (circular 
transformation)  have  been  defined.  In  order  to  have  the  robot  match  its  trajectory  to  the 
desired  path,  there  must  be  a  way  to  determine  how  far  the  robot  is  away  from  the  path 
and  how  to  proceed  to  the  path.  Just  as  the  driver  of  a  car  uses  the  steering  wheel  to 
match  the  vehicle's  trajectory  to  the  desired  path  (the  road),  the  robot  uses  a  steering 
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function  to  accomplish  this  task.  The  first  item  to  look  at  is  using  the  steering  function  to 
track  a  line  (see  Figure  8). 


To  generate  a  smooth  trajectory,  the  curvature  function  must  maintain  continuity. 
To  fulfill  this  requirement,  the  derivative  of  the  trajectory  curvature  must  remain  finite  at 
all  times  and  is  used  as  the  definition  of  the  steering  function  J(L): 

die 

—  =  J(L)  =  -aK-b<&(9-e,)-cM  (3.15) 

where  a,  b,  and  c  are  positive  constants,  <t>  is  an  angular  normalization  function 
(normalizes  angular  range  to  the  interval  \-n,  7t\ ),  0i  is  the  orientation  of  the  line  (L), 

and  Ad  is  the  signed  distance  from  the  line.  For  Ad,  positive  distances  are  to  the  left  of 
the  directed  line  and  negative  distances  to  the  right. 
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The  three  terms  on  the  right  hand  side  of  Equation  (3.15)  are  negative  feedback 
terms  and  the  coefficients  are  feedback  gain  parameters.  The  first  term  -aic  is  negative 
feedback  for  curvature  error  (ki  =  0).  The  second  term  -bO  is  negative  feedback  for  the 
orientation  error.  The  third  term  -cAd  is  negative  feedback  for  positional  error  (Ad  is 
measured  normal  to  L).  When  combined  in  this  way,  these  terms  allow  the  robot's 
trajectory  to  converge  to  the  line  (L)  without  overshoot  or  undershoot.  However,  this  is 
only  true  for  an  optimal  combination  of  the  gain  parameters  (a,  b,  c)  defined  below: 

a  =  3k 

b  =  3k2  (3.16) 

c- k3 

where  k  is  the  gain  of  the  steering  function.  See  section  3.2  of  Reference  [5]  for  a 
detailed  derivation  of  these  parameters. 

The  smoothness  of  the  line  tracking  motion  is  defined  by: 

*  =  j  (3.17) 

which  represents  a  smoother  motion  with  larger  o  and  a  sharper  motion  with 
smaller  a.  This  parameter  is  the  only  parameter  in  the  steering  function  J(L),  and  is  used 
to  control  how  fast  the  robot  reacts  to  the  feedback. 

Figure  9  illustrates  the  effect  of  different  c  values  on  the  robot's  trajectory  when 
the  robot  starts  at  q0  =  (0, 100, 0, 0)  and  tracks  the  x-axis,  L  =  (0, 0, 0, 0). 
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7.  Tracking  Circles 

The  steering  function  for  directed  circle  tracking  is  similar  to  that  of  Equation 
(3.15).  In  line  tracking,  the  curvature  of  the  path  (L)  was  zero.  With  circle  tracking,  the 
curvature  of  the  path  has  a  value  other  than  zero  and  must  be  taken  into  consideration. 
This  thesis  does  not  make  extensive  use  of  circle  tracking,  and  therefore  the  definitions  of 
the  steering  function  values  are  stated  below  without  detailed  explanation.  Figure  10 
depicts  the  parameters  involved  in  circle  tracking. 

The  steering  function  for  tracking  a  circle  (L)  is: 

die 

—  =  J(L)  =  -a(tc -at,)- 6O(0  -  0, )  -  cd'  (3.18) 

Uj 
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Figure  10.  General  Circle  Tracking  From  Ref.  [2], 

where  ki  is  the  curvature  of  the  circle,  6i  is  the  tangential  orientation  of  the  closest 
point  on  the  circle,  and  d*  is  the  signed  distance  to  the  closest  point  on  the.  The  gain 
coefficients  are  also  effected  by  the  curvature  of  L  and  become: 

a  =  3k 

b  =  3k2-K]  (3.19) 

c  =  k3 -3kfc2 

References  [2,  6]  provide  an  in  depth  discussion  on  these  topics. 

8.  Tracking  Paths 

The  steering  function  presented  in  the  previous  two  sections  tells  us  how  to  track  a 
single  directed  line  or  directed  circle.  A  path  can  be  defined  by  a  sequence  of  path 
segments  (lines  and  circles),  such  as  the  square  path  in  Figure  6.  When  changing  between 
the  current  path  segment  and  the  next  sequential  path  segment,  a  neutral  switching  point 
is  defined  to  prevent  unnecessary  early  turns  or  overshoots.  This  is  accomplished  by 
maintaining  two  steering  function  values,  J(Lf)  for  the  current  path  segment  and  J(Li+i) 


33 


for  the  next  path  segment.  The  neutral  switching  point  occurs  when  these  two  values  are 
equal,  J(Lj)  =  J(Li+i).  Since  these  steering  values  are  floating  point  numbers,  the  check  is 
made  in  program  code  by  monitoring  the  sign  of  J(Lj)  -  J(Li+i).  When  the  sign  changes, 
the  neutral  switching  point  has  been  reached.  This  approach  avoids  checking  the 
equivalence  of  two  floating  point  numbers,  which  can  lead  to  errors.  The  algorithm 
shown  below  demonstrates  how  neutral  switching  is  implemented  in  Yamabico  when 
following  a  defined  path  P: 

l-tf  =  4o 

2. P  =  (L1,L2,...,L„),n>l 

3.  do  forever 

4.  if  (n  >  2  and  (the  sign  of  J{LX )  -  j(L2 )  has  changed)) 

5.  then  shift  pathsegments(Z,,  <-  L2 ,  L2  <-  L3 , . . .) 

6.  ra  =  ra  +  j(Z,,)As 

7.  q~q°  C(  As, rads’) 

C.  THESIS  RELATED  MML  ROUTINES 

The  previous  section  (Section  B)  showed  how  the  transformation  model  of  motion 
can  be  used  for  path  planning,  trajectory  calculation,  and  relative  positioning  computation 
of  objects  in  the  world  of  a  rigid  body  robot.  This  section  discusses  how  this  model  is 
used  in  MML  to  direct  the  motion  of  Yamabico. 

1.  Motion  Modes 

Yamabico  is  primarily  controlled  by  two  types  of  motion  modes.  The  first  of 
these  is  a  track  mode  and  the  second  a  rotate  mode.  The  track  mode  is  further  broken 
down  into  two  types,  the  normal  track  mode  in  which  lines  and  circles  are  tracked  in 
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sequence,  and  the  track  stop  mode  where  the  current  line  or  circle  is  tracked  while  the 
robot  comes  to  a  halt.  The  rotate  mode  is  simply  a  turn  in  place  type  of  motion. 

By  using  these  modes,  many  types  of  paths  can  be  constructed  for  and  followed  by 
Yamabico.  The  user  specifies  the  path  by  issuing  a  sequence  of  track  and/or  rotate 
commands  in  the  user  control  file.  For  each  track  or  rotate  command,  a  corresponding 
path  element  is  inserted  into  an  instruction  buffer  (or  queue)  to  be  extracted  by  the  motion 
control  routines.  Figure  1 1  presents  a  block  diagram  of  this  process. 


Figure  11.  Motion  Control  Setup  After  Ref.  [1] . 


The  process  begins  in  the  file  <user.c>,  included  in  Appendix  B.  The  user 
defines  a  sequence  of  configurations  (x,  y,  0,  k)  and  issues  a  track  (function  track( ))  or  a 
rotate  ( Rotate( ))  command  for  each  configuration.  The  final  configuration  in  the 
sequence  is  issued  with  a  track  stop  (tracks  ( ))  command  so  that  motion  will  stop  when 
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the  sequence  is  complete.  The  track( )/Rotate( )/trackS( )  command  functions  are  located 
in  the  file  <seqcmd.O  (Appendix  A).  They  take  each  configuration  and  add  path  type 
information,  steering  related  information,  and  circle  information  if  the  configuration 
specifies  a  circle.  The  resulting  path  element  is  stored  in  a  data  structure  called  a  LINE, 
defined  in  <definitions.h>  (Appendix  A).  This  LINE  is  then  inserted  into  the  instruction 
buffer  via  the  function  AddLinef ),  located  in  <queue.O  (Appendix  A). 

This  process  does  not  directly  control  Yamabico's  motion.  In  other  words,  the 
sequence  of  configurations  is  a  list  of  directions  for  Yamabico  to  follow  in  the  future. 

The  motion  control  routines  in  <motion.O  (Appendix  A)  are  the  ones  that  control 
Yamabico.  They  receive  the  instructions  from  the  buffer  and  carry  them  out  according  to 
an  algorithm  like  the  one  mentioned  in  Section  B.8.  The  algorithm  is  actually  spread  out 
over  several  functions  and  files.  Lines  1  and  2  of  that  algorithm  are  executed  by  the  user 
program.  The  initial  configuration  is  either  specified  by  the  user  or  a  default  value  is 
used.  The  path  P  is  specified  by  the  instruction  buffer  built  by  the  track  and  rotate 
commands  and  the  current  path  element  is  kept  in  the  static  variable  currentPath  local  to 
the  file  <motion.c>. 

The  loop  in  lines  3-7  of  the  algorithm  is  executed  once  every  motion  control  cycle 
(10  ms).  When  the  system  signals  a  motion  interrupt,  the  motion  interrupt  handler  is 
called.  This  function,  MotionSysControl Q  in  <motion.c>,  is  the  workhorse  of  the  motion 
control  system.  It  first  updates  Yamabico's  movement  by  calculating  how  far  each  wheel 
has  traveled  during  the  last  motion  control  cycle.  It  then  uses  this  information  to  compute 
its  path  change  (As)  and  orientation  change  (A0). 
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After  updating  a  variable  keeping  track  of  the  total  distance  the  robot  has  traveled 
the  MotionSysControlO  function  calls  the  localize^  function  which  uses  the  calculated  As 
and  A0  to  update  the  vehicle's  current  configuration  by  circular  transformation.  The 
localization  corresponds  to  line  7  of  the  algorithm.  According  to  the  algorithm,  this  step 
is  out  of  order.  This  is  due  to  the  fact  that  As  and  A0  are  computed  directly  from  the 
hardware  and  can  be  used  immediately.  The  ordering  difference  is  negligible  since  k 
changes  only  a  small  amount  during  the  motion  control  cycle. 

The  next  step  in  MotionSysControlO  calculates  the  actual  linear  and  rotational 
velocities  maintained  by  the  robot.  These  are  used  in  later  calculations.  At  this  point  all 
motion  data  from  the  last  motion  control  cycle  has  been  calculated  and  LogMotionO  is 
called  to  log  the  motion  data  if  logging  has  been  turned  on. 

MotionSysControlO  next  calls  motionRulesO  to  determine  the  commanded  linear 
and  rotational  velocities  for  the  next  motion  control  cycle.  The  motionRulesO  function 
performs  all  the  calculations  involving  future  movements.  It  encapsulates  the 
functionality  of  lines  4-6  in  the  algorithm.  Depending  on  the  motion  mode  (track  or 
rotate),  it  directs  program  execution  to  an  appropriate  motion  rule  handling  function.  The 
motion  rule  function  (trackRule( )  or  rotateRule( ))  is  responsible  for  performing  steering 
function  calculations,  extracting  the  next  path  element  from  the  buffer  if  appropriate, 
updating  k,  and  returning  the  commanded  velocities  for  the  next  motion  control  cycle. 

The  returned  commanded  velocities  are  relayed  back  through  the  motionRules( ) 
function  to  the  MotionSysControlO  function  where  they  are  translated  into  commanded 
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velocities  for  each  wheel  by  the  function  SetMovement( ).  Once  this  is  done,  the  system 
clock  is  updated  and  an  external  LED  is  blinked  for  visual  feedback.  This  completes  one 
motion  control  interrupt  cycle  and  control  is  returned  back  to  the  main  system  where 
other  computations  are  performed  or  other  interrupts  are  handled. 

2.  Linear  Fitting 

Each  sonar  on  Yamabico  is  capable  of  gathering  and  maintaining  collections  of 
data  in  both  raw  form  (distance  from  sonar)  and  in  global  form  (global  position  of  the 
return).  The  related  data  structures  are  contained  in  the  file  <sonar.h>  (Appendix  A)  and 
related  functions  in  <sonarmath.c>  (Appendix  A) .  Some  of  these  functions  provide  the 
ability  to  fit  a  line  to  a  collection  of  global  positions  using  the  least-squares  summation 
technique. 


a)  Equal  Weighted  Points 

The  following  discussion  of  least-squares  linear  regression  analysis  is 
extracted  from  Ref.  [1]  and  follows  techniques  covered  in  most  probability  and  statistics 
textbooks,  such  as  Ref.  [7]. 

For  a  set  of  equally  weighted  unique  points,  a  line  that  best  fits  the  points 
can  be  defined  as  that  line  which  minimizes  the  sum  of  the  squares  of  the  distances  from 
the  points  to  the  line.  The  distance  of  a  point  to  the  line  is  called  the  residual  and  is 
denoted  by  8.  The  goal  is  therefore  to  minimize  the  sum  S  =  ^  S2  . 

First,  let  R  =  {pl,...,pn },  where  n  >  2  and  pi  =  (xj,  yO,  be  the  set  of  n 
points  to  be  fitted.  The  moments  1%  (0  <  j  +  k  <  2)  of  R  are  defined  as: 
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The  centroid  C  of  the  set  R  is  given  by: 
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In  finding  a  line  L  to  represent  a  best  fit,  a  parametric  representation  of  L 
is  adopted  using  the  line's  normal  direction  a  and  distance  r  from  the  origin  O.  This 
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approach  is  different  from  standard  linear  regression  analysis  but  has  the  advantage  of  not 
having  any  singularities  with  respect  to  vertical  lines.  The  line  is  therefore  defined  in 
terms  of  these  parameters,  L  =  (r,  a)  (see  Figure  12). 


Figure  12 .  Parametric  Representation  of  Linear  Fitted  Line 
After  Ref .  [2]. 

Note  that  every  point  (x,  y)  on  the  line  L  =  (r,  a)  satisfies  the  relation: 

x  cos  or +y  sin  a  =  r  (3.30) 

which  implies  that  the  residual  of  the  point  is: 

£  =  jtcosa  +  ysina-r  (3.31) 

Also  note  that  r  is  negative  if  the  projection  of  0  onto  L  is  in  the  second  or 
third  quadrant  (Figure  13). 

Finally,  the  sum  of  squares  of  the  residuals  becomes: 

S  =  ^S2  =  ^  ((x,  cosa+y,  sin  or)  -  r)2  (3.32) 

»=i  i=i 
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where  atan2()  is  a  more  robust  arctangent  function  found  in  most 
computer  language  libraries.  It  eliminates  quadrant  ambiguities  returning  an  angle  in  the 

range  [-%,  ji]  and  therefore,  by  Equation  (3.35),  a  is  restricted  to  the  range 

The  four  possible  quadrant  outcomes  for  L  are  depicted  Figure  13. 

The  linear  fitting  routines  in  Yamabico  use  this  procedure  to  calculate  L 
for  the  global  positions  of  sonar  returns.  Each  of  the  twelve  sonars  is  able  to  do  this 
independently  and  stores  current  linear  fitted  data  in  their  own  data  structure.  When  a 
new  sonar  return  is  received,  the  point  data  is  used  to  update  the  linear  fitted  data  by 
adding  it  to  the  moment  summations  in  Equations  (3.20-3.25).  For  instance,  let  (x„+i, 
yn+i)  be  the  new  point  and  m'jk  (0  <  j  +  k  <  2)  be  the  current  value  of  each  moment.  The 


new  moment  summations  mJk  would  then  be  calculated  as  follows: 


’00  =<  +1  =  «  +  1 

(3.36) 

ml0  =  m[ o  +xn+1 

(3.37) 
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Figure  13.  Four  Quadrant  Possibilities  for  Linear 
Fitted  Line. 


«oi  =<  +yn+\ 

(3.38) 

mn  =m'u  +x„+1yn+l 

(3.39) 

m20  =m'20+x2„+l 

(3.40) 

m<>2  =< 2  +yl  1 

(3.41) 

The  centroid  and  secondary  moments  are  then  recalculated  using  these 
new  values,  followed  by  a  recalculation  of  r  and  a  yielding  a  new  line  L.  When  the 
distance  from  a  new  point's  location  to  the  current  L  exceeds  a  threshold  value, 
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processing  of  the  current  line  segment  stops,  its  endpoints  are  stored,  the  moments  are 
reset,  and  calculation  of  a  new  line  segment  is  begun  using  the  new  point  data  as  the  first 
point. 

This  process  is  repeated  on  every  sonar  control  cycle  for  each  sonar 
enabled  for  linear  fitting.  The  building  of  line  segments  can  be  used  to  map  a  room, 
determine  the  shapes  of  objects,  or  update  navigation  by  comparing  the  line  segments  to  a 
map.  All  of  these  uses  are  passive  and,  except  for  the  last  one,  do  not  contribute  to  the 
robot's  motion  analysis. 

b)  Using  a  Decay  Factor 

In  order  to  follow  a  wall  with  sonar,  a  more  reactive  technique  must  be 
applied.  Through  conversations  with  Dr.  Kanayama  a  modified  version  of  the  standard 
least-squares  algorithm  was  developed  for  use  in  Yamabico.  This  modified  version  uses 
the  idea  of  a  decay  factor  applied  to  current  linear  data  when  adding  a  new  point. 

In  Equations  (3.36-3.41)  there  is  an  implicit  multiplicative  factor  of  one 
applied  to  the  m'jk  terms.  This  follows  from  the  stipulation  that  all  points  are  equally 

weighted.  If  this  factor  is  changed  to  a  value  less  than  one,  then  as  new  points  are  added, 
the  effect  of  previous  points  on  the  linear  data  would  decay  in  significance.  In  other 
words,  there  is  still  an  overall  history  of  previous  data  but  new  points  have  a  greater 
impact  on  the  calculation  of  the  line  L.  In  effect,  the  new  point  has  a  weight  of  one,  the 
previous  point  has  a  weight  of  DF  (where  DF  =  decay  factor),  the  point  prior  to  that  has  a 
weight  of  (DF)2,  etc.  Rewriting  Equations  (3.36-3.41)  to  reflect  this  change  results  in: 
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mw  -  DF x  m'w  +1  =  DFxn  +  \ 

(3.42) 

ml0  =  DF  x  m[0  +  x„+1 

(3.43) 

m0l  =  DFxm'0l+yn+l 

(3.44) 

mn  -  DF  x  m'n  +  x„+tyn+1 

(3.45) 

m20  =  DF  x  m'2()  +  x„2+I 

(3.46) 

m02  =DFxm'02+yl ri 

(3.47) 

Each  time  a  new  point  is  added,  the  moments  are  recalculated  in  this 
manner  and,  as  in  the  equal  weighted  procedure,  the  centroid,  secondary  moments,  r,  and 
a  are  recalculated  to  give  a  new  representation  for  the  line  L.  This  technique  yields  good 
response  to  a  changing  environment.  It  is  reactive  in  that  instead  of  having  to  start  a  new 
line  segment  if  a  new  point  is  beyond  a  certain  threshold,  as  mentioned  in  the  previous 
section,  the  line  L  is  adjusted  to  fit  the  new  data.  In  this  way,  the  line  constantly  changes 
and  adjusts  to  the  environment. 

Figure  14  shows  the  effect  of  three  different  decay  factors  (<  1)  compared 
to  no  decay  factor  (=  1)  on  the  linear  fitted  line.  It  simulates  a  robot  traversing  a  wall  and 
taking  sonar  readings.  The  wall  is  represented  by  the  connected  solid  line.  The  global 
position  of  the  sonar  return  from  the  wall  is  represented  by  the  intersection  of  the  dashed 
vertical  lines  and  the  wall.  The  orientation  of  the  linear  fitted  line  is  depicted  by  the  solid 
line  segments  at  the  top  of  the  dashed  lines. 

Analysis  of  Figure  14  results  in  the  conclusion  that  smaller  decay  factors 
provide  greater  response  to  new  data.  For  wall  following,  the  robot  should  react  to 
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changes  in  a  timely  manner  while  maintaining  a  smooth  and  continuous  path  curvature. 
Decay  factors  that  are  too  large  (i.e.  close  to  one)  violate  the  first  stipulation  while  decay 
factors  that  are  too  small  violate  the  second  stipulation.  It  was  decided  through  empirical 
observation  that  a  decay  factor  of  0.9  would  be  used  in  the  linear  fitting  with  decay  factor 
algorithms. 


\ 
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(tuo)x 


Figure  14.  Linear  Fitting  Decay  Factor  Effect. 
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Linear  Fitting  w/  Decay  =  0.30 


IV.  WALL  FOLLOWING  AND  OBSTACLE  AVOIDANCE 

The  foundation  for  solving  the  problem  defined  for  this  thesis  lies  in  having 
Yamabico  maintain  an  offset  from  a  wall  while  moving  in  the  forward  direction.  The 
first  attempt  in  realizing  this  goal  was  to  call  existing  MML  functions  from  the  user  file  to 
direct  Yamabico's  motion.  This  approach  proved  to  be  flawed  and  it  was  determined  that 
implementing  a  new  type  of  motion  mode  would  be  the  best  method  for  solving  the 
problem.  Implementation  of  this  new  motion  mode  required  modifications  to  the  MML 
software  library. 

This  chapter  discusses  why  the  first  solution  attempt  failed,  how  a  template  of  a 
new  motion  mode  was  added  to  MML,  and  how  this  template  was  filled  in  to  solve  the 
problem  of  following  a  wall. 

A.  FIRST  ATTEMPT 

Section  C.l  of  Chapter  HI  explained  how  currently  implemented  motion  modes 
are  used  to  define  a  path  that  Yamabico  will  follow.  For  instance,  if  the  user  wanted  the 
robot  to  follow  a  square  path,  such  as  the  one  described  by  Figure  6,  the  following  code 
would  be  created  in  the  <user.O  file: 
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CONFIGURATION  qO,  L,  T; 
int  n; 

L  =  qO  =  defineConf ig (0,  0,  0,  0)  ; 

T  =  defineConfig (100,  0,  PI/2,  0); 
setRobotConf iglmm (qO) ; 

for  (n  =  1;  n  <=  4;  i++)  { 

track (L) ; 

L  =  compose  (&L,  &T) ; 

} 

L  =  defineConfig (150,  0,  0,  0); 
tracks (L) ; 


In  this  example,  CONFIGURATION  is  a  data  structure  defined  in  <definitions.h> 
holding  configuration  parameters  (x,  y,  0,  k),  setRobotConfigImm( )  is  a  function  defined 
in  <motion.o  which  immediately  sets  the  robot's  configuration,  compose( )  is  a  function 
defined  in  <geometry.O  (Appendix  A)  implementing  the  composition  (o)  operation,  and 
track( )/trackS( )  are  the  functions  discussed  in  Chapter  HI.  The  for  loop  creates  the 
square  path  by  successively  calling  the  track()  function  with  the  current  line 
configuration  L,  which  adds  L  to  the  instruction  buffer,  and  then  composes  L  with  the 
transformation  T  to  get  the  next  line.  The  final  two  lines  of  code  create  the  last  line 
configuration  in  the  sequence  so  that  the  robot  will  stop  when  the  sequence  is  complete. 

This  procedure  works  well  for  predefined  paths  because  it  sets  up  the  instruction 
buffer  before  any  motions,  and  hence  motion  interrupts,  occur.  There  is  another  function 
defined  in  the  file  <queue.c>  called  FlushBuJferf ),  which  flushes  the  instruction  buffer. 
This  function  is  a  low  level  function,  similar  to  AddLine( ). 
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The  first  attempt  at  a  solution  for  wall  following  tried  to  use  these  low  level 
functions,  FlushBuffer( )  and  AddLine( ),  in  the  high  level  user  program.  The  idea  was  to 
track  an  initial  line  until  sufficient  linear  fitted  data  on  sonar  returns  from  the  wall  was 
acquired.  Once  the  robot  had  good  wall  data,  it  would  construct  a  line  configuration  from 
the  linear  data,  flush  the  buffer,  and  track  the  new  line  by  adding  it  to  the  instruction 
buffer.  This  solution  idea  was  flawed  because  of  timing  differences  between  the  motion 
interrupt  cycle,  the  sonar  interrupt  cycle,  and  the  user  program.  The  low  level  instruction 
buffer  routines  were  not  intended  to  be  used  in  the  high  level  user  routines.  This  fact  was 
not  originally  known  to  the  author,  but  was  realized  after  careful  analysis  of  the  code. 

The  problem  was  that,  once  robot  movement  commenced,  the  motion  interrupt 
routine  had  a  higher  priority  than  the  sonar  interrupt  routine  which,  in  turn,  had  higher 
priority  than  the  user  program  (Table  3).  The  only  possible  way  to  fix  this  problem  was 
to  disable  interrupts  (normally  bad  idea  in  interrupt  driven  programs)  during  critical 
sections  of  code  in  the  user  program.  However,  this  still  caused  problems.  If  a  disable 
interrupt  command  was  given  in  the  user  code,  there  was  no  guarantee  that  the  command 
would  be  carried  out  immediately  because  this  command  was  contained  in  a  low  priority 
section  of  the  code.  These  timing  problems  were  too  much  to  overcome  for  solving  a 
quick  reaction  type  of  problem  and  this  first  solution  attempt  was  discarded. 

B.  IMPLEMENTING  A  NEW  MOTION  MODE 

To  implement  a  reactive  type  of  motion  mode  in  MML,  the  use  of  an  instruction 
buffer  must  be  bypassed.  The  functionality  of  this  new  motion  mode  should  closely 
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mirror  that  of  the  tracking  motion  mode,  but  should  also  be  self-sufficient  without 
holding  path  elements  in  a  buffer.  In  other  words,  the  user  program  should  be  able  to  call 
a  routine  which  initializes  the  system  to  perform  the  tasks  of  the  new  motion  mode,  but 
the  tasks  should  be  performed  by  the  lower  level  functions. 

In  the  tracking  motion  mode,  the  user  calls  track( )/trackS( )  (in  <seqcmd.c>)  to 
initialize  the  system  by  building  the  instruction  buffer.  After  the  initialization  is 
complete,  the  motion  interrupt  handling  routine,  MotionSysControl( )  in  <motion.c>, 
calculates  commanded  velocities  for  the  robot  on  each  interrupt  cycle  by  calling 
motionRules( ).  This  function  determines  the  current  type  of  motion  mode  (e.g. 
TRACKMODE)  and  calls  the  appropriate  motion  rule  function,  such  as  trackRule( ).  The 
motion  rule  function  handles  the  details  of  steering  {steer ( ) ),  determining  the  next  path 
element  via  instruction  buffer  management  {ReadNextLine( ),  GetLine()  ),  and  current 
path  updating.  After  computations  are  complete,  commanded  velocities  are  returned  to 
the  motion  interrupt  handling  routine  and  are  applied  to  the  robot. 

The  new  motion  mode  should  behave  in  a  similar  manner.  The  user  should  be 
able  to  call  a  function  trackXXX(),  which  initializes  the  system  for  the  new  mode  (XXX). 
This  function  should  be  contained  in  a  file  like  <XXXcmd.c>.  After  initialization,  the 
motion  interrupt  handling  routine  would  call  motionRules( ),  as  before.  This  function 
should  then  choose  an  appropriate  motion  rule,  trackXXXRule( '),  based  on  the  current 
motion  type,  TRACKXXXMODE.  The  trackXXXRule( )  function  should  handle  the 
specific  steering  computations  {steerXXX( )),  update  the  current  path,  and  return 
commanded  velocities.  Note  that  since  this  will  be  a  reactive  motion  mode,  there  won't 
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be  a  next  path  element,  only  adjustments  to  the  current  path.  Figure  1 5  shows  a  block 
diagram  of  this  process. 


Figure  15 .  New  Motion  Mode  Process . 


The  following  sections  discuss  how  these  template  forms  (XXX)  were 
implemented  to  support  the  wall  following  motion  mode. 

1.  The  User  Program  Function  Call 

At  the  highest  level,  the  user  program  initializes  the  system  for  wall  following  by 
calling  the  function  trackWall(sonarNum,  offset).  This  function  is  used  to  specify 
whether  wall  following  will  occur  with  the  wall  on  the  left  side  or  the  right  side  of  the 
robot.  It  is  also  used  to  specify  what  the  offset  distance  will  be  from  the  wall  to  the  robot. 
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In  the  wall  following  mode,  the  robot  uses  linear  fitted  data  from  the  wall  as  its 
navigation  source.  The  trackWall( )  function  is  used  to  set  up  the  sonar  system  to 
perform  this  task.  The  user  is  able  to  specify  which  sonar  to  use  by  passing  the  sonar 
number  in  through  the  first  argument,  sonarNum.  This  has  the  added  effect  of  specifying 
which  side  the  wall  will  be  on  (#7  for  right  side,  #5  for  left  side). 

Constants  are  used  in  the  code  instead  of  sonar  numbers  to  improve  readability. 
These  constants  are  defined  in  the  file  <sonar.h>  and  have  the  form  5###,  where  the  S 
signifies  "sonar"  and  ###  is  a  three  digit  number  specifying  the  angle  of  the  sonar 
measured  clockwise  relative  to  the  front  of  the  robot.  For  example,  the  right  hand  sonar 
(#7)  is  defined  as  S090,  the  left  hand  sonar  (#5)  is  S270,  and  the  front  sonar  (#0)  is  S000. 

The  second  argument  to  trackWall( )  is  offset  and  is  used  to  specify  the  offset 
distance  the  robot  should  use  when  following  the  wall.  This  distance  is  the  magnitude  of 
the  offset  measured  from  the  center  of  the  robot  to  the  current  linear  fitted  data 
representing  the  wall. 

2.  The  trackWall( )  Function 

The  trackWall( )  function  is  located  in  the  new  MML  file  <wallcmd.c> 

(Appendix  B).  The  function  prepares  the  motion  system  and  the  sonar  system  for  wall 
following. 

For  the  motion  system  setup,  it  creates  and  sets  field  values  in  a  LINE  variable 
called  pathElement.  LINE  is  a  data  structure  containing  fields  for  the  motion  mode  type, 
a  configuration,  steering  function  gain  parameters,  circular  arc  parameters,  and  other 
motion  variables  and  is  defined  in  <definitions.h>. 
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Depending  on  the  sonar  number  argument,  trackWall( )  first  assigns  one  of  two 
values  to  the  Type  field:  TRACKRWALLMODE  for  following  a  wall  to  the  right  of  the 
robot,  or  TRACKLWALLMODE  for  following  a  wall  to  the  left  of  the  robot.  These  values 
are  enumeration  values  of  MODE,  which  is  defined  in  <definitions.h>.  The  offset  value 
is  also  set  at  this  point  depending  on  the  sonar  number.  If  the  sonar  number  is  S090  then 
die  wall  will  be  on  the  right  side.  This  implies  that  the  robot  will  be  to  the  left  of  the 
directed  line  representing  the  wall.  Recalling  the  discussion  in  Chapter  m  (Section  B.6), 
positive  distances  are  to  the  left  of  a  directed  line  and  negative  distances  are  to  the  right. 
When  the  sonar  number  is  S090,  the  offset  will  be  positive  because  the  robot  should 
maintain  the  offset  distance  to  the  left  of  the  wall.  On  the  other  hand,  when  the  sonar 
number  is  S270,  the  offset  will  be  negative  because  the  robot  should  maintain  the  offset 
distance  to  the  right  of  the  wall.  The  user  specified  offset  is  also  checked  to  make  sine  its 
value  will  not  cause  the  edge  of  the  robot  to  come  too  close  to  the  wall.  If  it  will,  then  a 
default  value  is  assigned. 

trackWallf )  next  initializes  pathElement 's  configuration  to  a  default  value  (all 
zeroes)  and  its  steering  gain  parameters  using  the  robot's  <r  value.  The  initial  path  will  be 
a  straight  line,  so  the  circle  parameters  of  pathElement  won't  initially  be  used  by  the 
motion  system.  To  be  safe,  the  center  point  is  set  to  zeroes.  The  radius,  however,  is  used 
to  pass  the  offset  value  into  the  motion  system.  Once  pathElement  has  been  set  up,  it  is 
used  as  an  argument  to  the  function  setPathElement().  This  function  is  defined  in 
<motion.c>  and  simply  copies  its  argument  into  currentPath,  a  static  file  scope  variable 
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used  by  many  of  the  motion  functions.  This  simple  functionality  is  important  because  it 
is  the  only  way  for  a  routine  outside  of  <motfon.O  to  set  currentPath. 

After  pathElement  and  currentPath  have  been  set,  trackWall( )  matches 
Yamabico's  initial  configuration  to  the  initial  path  segment  by  calling 
setRobotConfigImm( )  with  pathElemenfs  configuration  as  the  parameter.  This  function 
is  located  in  <motion.c>  and  immediately  sets  the  robot's  configuration  to  the  specified 
argument. 

Finally,  trackWall( )  sets  up  the  sonar  system  for  wall  tracking  by  enabling  the 
required  sonars  ( S000  for  both  left  and  right  wall  following  and  either  S090  or  S270 
depending  on  the  sonar  number  argument)  and  enabling  linear  fitting  using  a  decay  factor. 
The  functions  used  to  do  this,  EnableSonar( )  and  EnableLinearFittingDecay( ),  are 
located  in  files  <sonarcard.c>  (Appendix  A)  and  <sonarmath.c>  respectively. 
EnableSonar( )  is  a  hardware  related  function.  It  takes  a  sonar  number  as  an  argument 
and  adds  that  sonar  to  an  internally  stored  enabled  sonar  list. 

EnableLinearFittingDecayO  sets  up  the  specified  sonar  to  perform  the  linear  fitting  with 
decay  computations  discussed  in  Chapter  ID  (Section  C.2.b). 

At  this  point,  all  initialization  is  complete,  control  is  returned  to  the  user  program, 
and  the  motion  and  sonar  control  systems  begin  handling  movement  and  sonar  events. 

3.  The  wallHugRule( )  and  steerRyWall( )  Functions 

Once  motion  has  started,  the  motion  interrupt  handling  routine  will  call  the 
motionRules( )  function  to  obtain  commanded  velocities  for  the  next  motion  cycle.  In  the 
wall  following  mode,  this  function  will  in  turn  call  the  new  wallHugRule( )  function,  in 
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<motion.c>,  which  updates  the  current  path  element  and  calculates  the  commanded 
velocities  to  keep  the  robot  on  the  current  path.  The  wallHugRule( )  function  is 
essentially  the  heart  of  the  wall  following  motion  mode.  It  uses  both  the  currently 
implemented  steer ( )  function  and  a  new  steerByWall( )  function  to  calculate  commanded 
velocities.  The  steerByWall( )  function  calculates  steering  function  values  based  on  the 
wall  represented  by  the  decay  version  of  linear  fitting.  Both  of  these  new  functions  form 
the  foundation  for  solving  the  problem  presented  in  this  thesis  and  are  explained  in  more 
detail  in  the  following  sections. 

C.  FOLLOWING  A  STRAIGHT  WALL 

In  order  to  follow  a  wall  using  sonar,  the  first  thing  that  must  be  accomplished  is 
finding  a  way  to  represent  the  wall  based  on  the  sonar  returns.  Once  this  representation  is 
found,  the  next  step  is  to  find  a  way  to  direct  the  robot's  motion  so  that  it  maintain  a 
defined  offset  distance  from  the  wall. 

1.  Method  for  Calculating  Linear  Representation  of  Sonar  Returns 

Once  a  sonar  has  been  enabled  to  perform  linear  fitting  calculations,  it  calculates 
and  maintains  the  linear  data  in  a  structure  called  SEGMENT _RES_DEC. AY  which  is 
defined  in  <sonar.h>.  This  structure  keeps  all  the  important  data  for  the  linear  fitting 
with  decay  calculations  such  as  the  number  of  points  in  the  data  set,  the  moments  of  the 
set,  the  decay  factor,  and  the  parameters  r  and  a  that  define  the  linear  fitted  line.  It  also 
has  two  helper  fields,  one  containing  the  sonar  number  and  the  other  a  flag  indicating 
whether  the  linear  data  is  usable  or  not. 
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There  is  one  other  important  sonar  data  structure  called  SONARD,  also  defined  in 
<sonar.h>.  It  contains  all  necessary  sonar  data  for  a  particular  sonar  including  boolean 
fields  indicating  whether  normal  and/or  decay  linear  fitting  is  turned  on,  and  if  the  sonar 
data  has  been  updated  recently.  Other  fields  hold  data  related  to  the  current  sonar  return 
including  the  global  position,  current  and  previous  range,  and  the  robot's  configuration 
(position  and  orientation)  at  the  time  of  the  reading.  There  are  also  fields  containing  the 
sonar’s  configuration  relative  to  the  center  of  the  robot.  The  sonar  system  maintains  an 
external  table  of  these  SONARD  data  structures,  one  for  each  sonar  and  indexed  by  the 
sonar  number,  in  an  array  called  sonar  table [ ].  This  array  can  be  accessed  from  any  file 
that  includes  the  <sonar.h>  header  file. 

When  a  sonar  interrupt  occurs,  the  sonar  interrupt  handling  routine, 

Sonar SysControl( )  in  <sonar.c>  (Appendix  A),  is  invoked.  This  function  updates  the 
sonar  table [ ]  array  for  the  sonars  in  the  currently  active  sonar  group.  Of  particular 
interest  here  is  that  it  sets  the  update  field  to  1  (updated)  and  calls  the 
LinearFittingDecayO  function  if  the  decay  linear  fitting  flag  is  1  (enabled).  The  update 
field  is  used  to  alleviate  problems  associated  with  the  timing  differences  between  the 
motion  system  and  the  sonar  system  interrupts.  The  LinearFittingDecay( )  function  is 
used  to  update  the  linear  fitted  line  representing  the  sonar  data  (i.e.  the  wall).  This 
function  is  located  in  <sonarmath.c>.  It  does  not  actually  do  the  linear  calculations  but 
is  used  to  perform  error  checking  before  calling  the  AddToSegmentDecay( )  function, 
which  does  the  work. 
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AddToSegmentDecay( )  is  also  located  in  <sonarmath.c>  and,  when  called,  adds 
the  new  sonar  return  to  the  linear  data  and  recomputes  die  linear  fitted  line.  The 
arguments  to  this  function  are  the  sonar  number  and  the  global  position  of  the  new  sonar 
return.  The  sonar  number  is  used  as  an  index  into  the  array  seg_data_decay[  J  which  is  a 
static  file  scope  array  holding  the  SEGMENT  RES  DECAY  structures  for  each  sonar. 

The  function's  second  argument  is  the  point  that  will  be  added  to  the  linear  data.  If  the 
addition  of  the  new  point  will  create  a  data  set  with  two  or  more  points, 
AddToSegmentDecay( )  adds  the  new  point  to  the  existing  linear  moments  using  the 
procedure  described  by  Equations  (3.42  —  3.47),  recalculates  the  centroid  and  secondary 
moments  using  Equations  (3.26  -  3.29),  recomputes  the  line  representation  parameters 
using  Equations  (3.34)  and  (3.35),  and  sets  the  usable  field  to  1  (true).  If  the  new  point  is 
the  first  point  in  the  data  set,  the  function  only  calculates  the  linear  moments  and  the 
usable  field  will  remain  0  (false). 

Additional  functions  are  provided  in  <sonarmath.c>  to  access  or  set  various 
components  of  the  arrays  of  structures  associated  with  the  linear  fitting  with  decay 
algorithm.  Each  of  these  functions  takes  the  sonar  number  as  one  argument  since  they 
reference  arrays  that  are  indexed  by  the  sonar  number.  Table  4  contains  a  list  of  these 
functions,  the  array/structure  and  field  they  access,  and  a  short  description. 

2.  Using  Linear  Fitted  Data  to  Represent  Wall 

The  sonar  system  interrupt  handler  ensures  linear  data  is  kept  up  to  date  for  those 
sonars  that  have  been  enabled  for  linear  fitting.  (NOTE:  From  this  point  forward,  any 
reference  to  linear  fitting,  linear  data,  etc.  will  mean  data  associated  with  the  decay 
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version  of  the  linear  fitting  algorithm.)  Any  function  may  obtain  a  current  copy  of  the 
data  by  calling  GetSegmentDecay(sonarNum).  This  function  returns  a  copy  of  the  current 
SEGMENT _RES_DECAY  structure  associated  with  the  specified  sonar.  Before  using  the 
data  in  this  structure,  it  should  be  checked  for  usability  by  testing  the  usable  flag.  If 
usable,  the  r  and  a  values  specify  the  line  representing  the  data  points.  These  values 
specify  the  distance  and  normal  direction  to  the  line  from  the  global  origin.  The  line 
representation  specifies  an  undirected  line.  In  order  to  represent  a  wall,  the  line  must  be 
converted  to  a  directed  line,  such  as  a  line  configuration. 


FUNCTION 

Array 

Structure 

Field 

Purpose 

EnableLinearFittingDecay (  ) 

sonar  table [  ] 
SONARD 

decayFitting 

Sets 

decayFitting 
to  1 

(enabled) 

DisableLinearFittingDecay (  ) 

sonar  table [  ] 
SONARD 

decayFitting 

Sets 

decayFitting 
to  0 

(disabled) 

GetSeginentDecay  (  ) 

seg  data  decay [  ] 
SEGMENT_RES_DECAY 

Entire 

structure 

Returns  a 
copy  of  the 
structure 

SetDecayFactor (  ) 

seg  data  decay [  ] 
SEGMENT_RES_DECAY 

decayFactor 

Sets 

decayFactor 
to  a 

specified 

value 

ResetMomentsDecay (  ) 

seg  data  decay [  3 
SEGMENT_RES_DECAY 

Entire 

structure 

Resets  all 
fields  to 
default 
values  and 
usable  to  0 
(false) 

Table  4 .  Access  and  Set  Functions  of  Linear  Fitting  w/  Decay 
Algorithm . 


To  convert  the  undirected  line  to  a  line  configuration  representing  the  wall,  values 


for  the  x  and  y  coordinates,  orientation  ((3),  and  curvature  (k)  must  be  found.  Since  it  is  a 
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line,  its  curvature  will  be  zero.  The  x  and  y  coordinates  are  easily  converted  from  r  and  a 
through  the  relations: 

x  =  rcosa  (4.1) 

y  =  r  sin  a  (4.2) 

The  wall's  orientation  (p)  depends  on  the  robot’s  orientation  (0).  The  two  possible 
values  for  P  are  (a  +  ji/2)  and  (a  -  rc/2).  Figure  16  shows  examples  of  both  situations. 


To  determine  the  proper  choice,  one  of  these  values  is  compared  to  0.  Touring 
wall  following,  the  robot's  orientation  will  be  close  to  the  wall's  orientation  and  definitely 
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less  than  90°.  Therefore,  the  correct  choice  is  found  by  comparing  the  magnitude  of  the 
difference  between  the  two  possible  values  and  the  robot's  orientation: 
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where  0( )  is  the  normalization  function  for  angles. 

The  wall  configuration  has  now  been  extracted  from  the  sonar  data  and  can  be 
defined  as  (x,  y,  p,  0). 

3.  Following  the  Wall 

In  order  to  follow  a  wall  using  sonar,  the  procedure  described  in  the  last  section  is 
implemented  each  time  new  sonar  data  is  available.  In  the  wall  following  motion  mode, 
the  wallHugRule( )  function  is  called  during  every  motion  control  cycle.  This  function  is 
responsible  for  maintaining  the  current  path  element  and  calculating  the  commanded 
velocities  that  will  keep  the  robot  on  this  path. 

Since  the  motion  interrupt  handler  runs  approximately  five  times  between  sonar 
interrupts  when  one  sonar  group  is  active,  calculating  a  new  configuration  for  the  wall 
will  only  occur  when  new  sonar  data  is  available.  Each  time  wallHugRule( )  is  called,  it 
first  checks  if  the  sonar  data  has  been  updated.  If  not,  the  current  wall  configuration 
remains  unchanged.  If  new  sonar  data  has  been  received,  then  the  function  calculates  the 
new  wall  configuration  using  the  procedure  described  in  the  last  section. 
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The  current  path  is  calculated  using  the  wall  configuration  and  the  specified  offset 
distance.  As  stated  in  Section  B.2,  the  offset  value  is  passed  into  the  motion  system  by 
the  initialization  function  trackWall(),  through  the  radius  field  of  the  path  element.  The 
first  time  wallHugRule( )  is  invoked,  the  offset  value  is  stored  in  the  function  scope  static 
variable  offset.  The  current  path  is  computed  by: 

current  path  -  wall  config  °  (0,  offset,  0,0)  (4.4) 

This  places  the  current  path  parallel  to  the  wall  and  on  the  appropriate  side  at  the 
correct  distance  since  offset  is  negative  for  left  wall  following  and  positive  for  right  wall 
following. 

The  commanded  velocities  are  found  by  using  the  function  steerByWall( ).  This 
function  is  called  with  the  robot's  configuration,  the  wall  configuration,  and  the  offset.  It 
returns  a  steering  function  value  which  is  used  to  calculate  the  commanded  velocities.  It 
is  identical  in  functionality  to  the  normal  steer ( )  function  for  a  line.  It  uses  the  distance 
to  the  wall  minus  the  offset  distance  as  the  distance  feedback  and  the  difference  between 
the  robot's  orientation  and  the  wall's  orientation  as  the  angular  feedback. 

The  process  described  in  this  section  occurs  during  each  motion  control  cycle  and 
produces  the  desired  effect  of  following  a  wall  with  a  sonar.  The  "straightness"  of  the 
wall  can  vary  slightly  without  loss  of  wall  following  ability  because  the  linear  fitting  with 
decay  algorithm  makes  timely  adjustments  to  the  linear  representation  of  the  wall  and  the 
robot  adjusts  its  path  accordingly. 
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D.  ORTHOGONAL  WALLS  AND  OBSTACLE  AVOIDANCE 


Once  the  straight  wall  following  algorithm  was  created,  it  was  used  to  expand  the 
solution  scope  to  include  following  walls  with  orthogonal  turns.  To  solve  this  problem 
the  robot  uses  the  wall  following  algorithm  while  following  straight  wall  segments  until  a 
turn  is  sensed.  Turns  are  either  concave  (inside)  or  convex  (outside).  When  a  turn  is 
required,  the  robot  stops  executing  the  wall  following  algorithm  and  enters  a  multi-phase 
process  using  lines,  circular  arcs,  and  the  normal  steer ( )  function  to  navigate  inside  or 
around  the  comer  of  the  wall.  When  the  robot  is  abreast  of  a  new  wall,  it  returns  to  the 
wall  following  mode. 

The  sections  that  follow  explain  how  turns  are  sensed,  how  the  turn  is 
accomplished,  and  how  the  determination  is  made  to  return  to  wall  following. 

1.  Concave  Corners 

Concave  comers  are  sensed  by  Yamabico's  forward  looking  sonar  (S000).  The 
criteria  used  to  start  an  inside  turn  comes  from  the  neutral  switching  point  idea  discussed 
in  Chapter  IQ  (Section  B.8).  When  the  robot  is  following  a  wall  and  will  make  an  inside 
turn  to  the  next  wall,  the  next  path  will  90°  to  the  left  or  right  of  the  current  path.  Figure 
17  depicts  this  situation  when  the  robot  is  in  the  left  wall  following  mode. 

The  next  path  will  also  be  offset  from  the  next  wall  toward  the  robot's  current 
position.  In  normal  path  tracking  mode,  the  neutral  switching  point  occurs  when  the 
steering  function  for  the  current  line  equals  the  steering  function  for  the  next  line. 

Because  the  difference  in  angles  between  the  current  and  next  segment  is  known,  an 
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Figure  17 .  Neutral  Switching  Point  from  Current  Path  to 
Next  Path. 

expression  can  be  found  for  the  optimal  distance  to  start  a  turn.  This  is  accomplished  by 
equating  the  two  steering  functions  and  solving  for  distance.  The  expression  yielded  by 
this  was  solved  by  Dr.  Kanayama  [2]: 

d=  3aa  (4.5) 

where  d  is  the  optimal  turn  distance  (signed),  a  is  the  angular  difference  (7t/2),  and 
a  is  the  sigma  value  used  in  the  turn. 

The  forward  sonar  is  used  to  sense  when  this  distance  occurs.  However,  since  the 
next  path  will  be  offset  from  the  wall  and  the  sonar  senses  the  wall,  not  the  path,  the 
distance  at  which  the  robot  should  start  its  turn  is: 


63 


(4.6) 


d  =  3^a+\offset\ 

When  the  forward  sonar  senses  a  distance  less  than  or  equal  to  this  value,  the 
robot  stops  following  the  current  wall  (turns  the  side  sonar  off),  computes  an  approximate 
next  path  line  (L),  sets  the  current  path  to  L,  and  tracks  the  new  current  path  using  the 
normal  steer( )  function.  This  next  line  is  computed  relative  to  the  robot's  configuration 
by: 

L  =  robot  config  °  (drs/000  -  \offset\,0,  AOfi)  (4.7) 

where  distOOO  is  the  forward  sonar  distance  to  the  wall  and  A0  is  ±n/2  depending 
on  which  side  the  wall  is  on. 

While  in  the  turn  toward  the  new  current  path,  the  robot  orientation  is  compared 
to  that  of  the  current  path.  Once  this  difference  is  less  than  a  specified  value  (~1 0°),  the 
robot  is  in  a  transition  area.  The  side  sonar  is  turned  on  and  linear  data  for  that  sonar  is 
reset  via  ResetMomentsDecay( ).  While  in  this  transition  area,  the  robot  continues  to 
track  the  current  path  until  sufficient  linear  data  has  been  built  for  the  next  wall. 

When  there  are  enough  data  points  in  the  linear  fitting  algorithm,  the  robot  returns 
to  the  wall  following  mode. 

Figure  18  shows  the  phases  of  a  concave  turn.  To  implement  these  phases, 
additional  code  was  incorporated  in  the  function  wallHugRule( ).  Static  flag  variables 
were  added  to  perform  checks  on  turn  phase  status  (in  turn,  in  transition)  and  turn  type 
(inside,  outside).  A  non-static  flag  was  also  added  to  determine  the  track  type  (following 
wall,  current  path).  This  flag  is  set  each  time  the  function  is  called  and  is  used  to 
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distinguish  which  type  of  calculations  to  use,  the  wall  following  algorithm  or  the  normal 
path  tracking  algorithm. 
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Figure  18.  Phases  of  a  Concave  (Inside)  Turn. 


2.  Convex  Corners 

Convex  comers  are  sensed  by  the  side  sonar.  As  the  robot  proceeds  along  a 
straight  wall,  sonar  distance  readings  vary  only  slightly.  When  the  robot  moves  past  a 
convex  comer,  the  sonar  distance  measurement  will  jump  from  a  fairly  stable  reading  to  a 
much  larger  distance.  A  convex  turn  is  started  when  the  difference  between  the  current 
and  previous  sonar  readings  exceeds  a  certain  threshold  value. 

The  sequence  of  turn  phases  that  occur  during  a  convex  turn  are  similar  to  those 
of  the  concave  turn,  but  with  an  extra  phase.  When  the  threshold  value  is  exceeded,  the 
side  sonar  is  turned  off,  the  current  path  is  changed  to  a  circular  arc,  and  the  robot  tracks 
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the  current  path  around  the  comer.  The  circular  arc  is  defined  not  only  with  a 
configuration,  but  also  with  a  center  position  and  radius.  The  arc's  configuration  is 
defined  using  the  robot's  current  position  and  orientation  with  the  curvature  equal  to  the 
negative  reciprocal  of  the  offset  value: 


current  path  config  = 


robot  x,  robot  y,  robot  0,- 


ojfsetj 


(4.8) 


This  gives  the  circular  arc  the  proper  direction  (clockwise,  counter-clockwise) 
with  a  radius  equivalent  to  the  offset  distance.  The  current  path  structure  also  maintains  a 
radius  field  and  a  center  position  field.  These  fields  were  unimportant  in  the  wall 
following  and  concave  turn  implementations  because  only  line  paths  were  used,  but  must 
be  set  in  this  situation.  The  radius  value  is  simply  the  reciprocal  of  the  curvature.  The 
center  position  is  found  by  extracting  the  position  from  the  following  composition: 


current  path  config  °  (0 -offset  (4.9) 

The  robot  tracks  the  current  path  (arc)  for  approximately  90°  which  should 
theoretically  put  it  abreast  the  wall  comer  heading  in  the  proper  direction  for  the  next 
wall.  To  allow  for  motion  errors,  however,  the  current  path  is  set  to  a  line  configuration 
starting  at  the  robot's  current  position  with  the  robot's  orientation.  This  causes  the  robot 
to  track  straight  ahead  from  its  current  position. 

The  robot  tracks  the  current  path  (line)  for  a  specified  distance  to  avoid  the  two 
situations  depicted  in  Figure  19. 
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Figure  19.  Undesirable  Possibilities  of  a  Convex  (Outside) 
Turn. 


To  determine  when  the  specified  distance  has  been  traversed,  the  relative 
configuration  of  the  robot  to  the  current  path  is  computed  by: 

relative  config  =  [current path  config)~l  o  robot  config  (4. 1 0) 

When  the  x  coordinate  of  this  relative  configuration  is  greater  than  the  specified 
distance,  the  robot  enters  a  transition  area  similar  to  the  one  specified  in  the  concave  turn. 
The  side  sonar  is  turned  on  and  the  linear  data  for  that  sonar  is  reset.  While  new  linear 
data  is  being  built,  the  robot  continues  to  track  the  current  path. 

After  sufficient  linear  data  has  been  accumulated,  the  robot  returns  to  the  wall 
following  mode  and  tracks  the  new  wall. 

Figure  20  shows  the  phases  of  a  convex  turn.  To  implement  these  phases,  code 
was  again  added  to  the  function  wallHugRule( ).  The  static  flag  variables  continued  to  be 
used  to  perform  turn  phase  and  turn  type  checks.  An  extra  value  was  included  for  the 
turn  type  flag  to  represent  when  the  robot  is  tracking  the  circular  arc.  The  track  type  flag 
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Figure  20.  Phases  of  a  Convex  (Outside)  Turn. 


was  not  changed.  Table  5  shows  the  values  of  the  flags  for  the  different  phases  in 
concave  and  convex  turns. 

3.  Obstacle  Avoidance 

In  the  wall  following  mode,  any  obstacle  in  the  robot's  path  is  inherently  close  to 
the  wall.  By  implementing  the  capability  to  navigate  by  following  a  wall  and  around 
orthogonal  comers,  obstacle  avoidance  is  automatically  included.  The  obstacle  can  be 
incorporated  as  part  of  the  wall  (Figure  21)  and  the  robot  uses  the  new  capabilities  to 
steer  around  it. 
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Table  5 .  Values  of  Flag  Variables  in  wallHugRule (  )  During 
Phases  of  Turns . 


Figure  21.  Examples  of  Obstacles  in  Wall  Following  Path. 
4.  Analysis  of  the  wallHugRule( )  Function 


As  previously  stated,  the  wallHugRule( )  function,  located  in  <motion.O,  is  the 
heart  of  the  wall  following  motion  mode.  It  is  called  by  the  motionRules( )  function  on 


69 


every  motion  cycle  when  the  wall  following  mode  is  in  use  and  returns  the  commanded 
velocities  that  keep  the  robot  on  the  correct  path.  The  function  handles  all  the  phases  of 
wall  following  (straight  wall  following,  concave  turns,  convex  turns)  discussed  in  this 
chapter.  Because  this  function  is  so  important,  a  detailed  analysis  of  the  code  is 
warranted.  The  code  for  this  function  is  listed  in  Appendix  C  with  reference  line 
numbers.  In  die  analysis  that  follows,  line  numbers  will  be  indicated  by  specific 
reference  (e.g.  line  ##)  or  by  a  number  in  parentheses. 

Line  9  is  the  beginning  of  the  function  header.  It  specifies  that  the  function  will 
return  the  commanded  velocities  (linear  and  rotational)  in  a  structure  called  VELOCITY. 
The  function  accepts  as  arguments  (line  10)  the  actual  and  commanded  velocities  used  by 
the  robot  during  the  previous  motion  control  cycle. 

Lines  13-48  declare  the  variables  used  in  the  function.  The  CONFIGURATION 
variables  are  used  to  define  and  manipulate  wall  representation,  offset ,  relative 
positioning ,  inverse,  and  temporary  configurations  during  code  execution.  Linear  fitted 
data  from  the  sonar  is  extracted  into  the  curSeg  variable.  Wall  configuration  components 

are  stored  in  the  static  wall _ variables  defined  in  line  16  and  the  previous  angle  of  the 

wall  is  kept  in  the  static  variable  prevWallTheta  (line  17).  Line  19  defines  a  static 
variable  to  store  the  initial  orientation  value  of  a  circular  arc.  The  deltaDistance  variable 
is  used  in  the  final  computation  of  the  commanded  velocities  and  deltaAngle  is  used  to 
determine  which  way  the  robot  will  turn  in  various  situations  depending  on  which  side 
the  wall  is  on.  While  in  a  turn,  the  robot's  sigma  value  differs  from  the  value  used  for 
straight  wall  following.  savedSigma  saves  the  old  value  while  the  robot  turns. 
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pingedOnce  and  pingedTwice  are  flags  used  to  ensure  good  history  data  from  the  sonar. 
firstTimeCalled  is  only  true  the  first  time  the  function  is  called.  The  next  two  lines  (28, 
29)  declare  variables  that  hold  the  side  sonar  number  and  sonar  distances,  offset  stores 
the  required  offset  distance  the  robot  will  maintain  from  the  wall.  This  value  is  negative 
for  left  wall  following  and  positive  for  right  wall  following.  Lines  37-43  define  the  flag 
variables  which  are  used  to  specify  which  motion  state  the  vehicle  is  in.  The  first  of 
these,  halted,  is  used  in  collision  detection.  The  next  four  are  the  turn  phase  indicators 
depicted  in  Table  5.  The  final  three  function  variables  are  used  in  steering  calculations. 
jL  holds  the  steering  function  value,  kk  and  kk2  hold  the  current  path's  kappa  and  kappa 
squared  values,  and  k  is  the  steering  feedback  gain  parameter. 

After  the  variable  declarations,  the  function  first  tests  which  type  of  wall 
following  (left  or  right)  is  in  use  through  the  currentPath  structure.  This  structure  is  a 
static  file  scope  variable  which  stores  the  current  path  information  during  each  motion 
control  cycle.  The  switch  statement  in  lines  53-66  assigns  the  appropriate  values  to 
sonarNum  and  deltaAngle  depending  on  the  wall  following  mode. 

It  was  stated  earlier  that  the  offset  value  is  passed  into  the  motion  control  system 
through  the  radius  field  of  currentPath.  This  takes  place  in  lines  69-71.  When  the 
function  is  first  called,  the  offset  value  is  copied  from  currentPath.Rad.ius  into  offset. 

Line  74  extracts  the  linear  fitted  data  from  the  sonar  system  and  places  it  in 
curSeg  using  the  <sonarmath.O  function  GetSegmentDecay().  Once  this  data  available, 
lines  77-82  calculate  the  orientation  of  the  wall  using  Equation  (4.3). 
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Before  any  motion  calculations  take  place,  lines  86-93  check  for  a  possible 
upcoming  collision.  If  the  forward  sonar  has  been  updated  and  the  distance  it  measures  is 
less  than  a  predefined  threshold,  the  halted  flag  is  set,  commanded  velocities  are  set  to 
zero,  and  the  function  returns. 

To  ensure  good  sonar  data  history,  lines  96-104  determine  if  the  side  sonar  has 
been  updated  (pinged)  zero,  one,  or  two  times  and  sets  the  flags  pingedOnce  and 
pingedTwice  appropriately. 

The  next  section  of  code  (1 12-247)  performs  the  motion  phase  determination  and 
adjusts  the  currentPath  to  keep  the  robot  on  course.  The  outer  if  statement  breaks  down 
the  phases  into  blocks  for  wall  following  (1 12-180),  in  turn  (183-230),  and  in  transition 
area  (233-247). 

If  the  inTurn  flag  is  not  set,  the  robot  is  in  the  wall  following  mode,  and  the  first 
code  block  is  entered.  The  code  in  this  block  first  sets  the  sonar  distance  variables.  The 
front  sonar  distance  is  compensated  for  displacement  from  the  robot  center  (115).  The 
current  and  previous  sonar  readings  are  taken  in  raw  form.  These  values  are  used  in  the 
nested  if  statement  to  check  for  turn  indications.  The  if  statement  on  lines  121, 122  first 
checks  that  the  sonar  has  been  updated  and  then  uses  the  criteria  described  by  Equation 
(4.6)  to  determine  if  the  robot  should  begin  an  inside  (concave)  turn.  When  the  criteria  is 
met,  the  inTurn  flag  is  set,  the  turnType  is  set  to  the  value  for  an  inside  turn,  and  the 
current  sigma  value  is  saved  and  then  set  to  a  predefined  value.  Lines  127-131  update  the 
currentPath  configuration  using  the  model  described  in  Equation  (4.7).  Because  the 
sigma  value  was  changed,  lines  132-135  update  the  steering  parameters  of  currentPath  to 
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reflect  the  change.  Finally,  because  the  robot  is  now  in  a  turn,  the  trackType  flag  is  set  to 
1  to  signify  that  the  normal  steer  ( )  function  should  be  used  to  track  the  current  path.  The 
next  two  lines  finish  the  code  block  by  resetting  the  sonar  update  field  and  disabling  the 
side  sonar. 

If  the  criteria  for  an  inside  turn  is  not  met,  the  code  next  checks  for  an  outside 
turn.  Lines  144-146  check  if  the  sonar  has  been  updated  and  has  good  history  data,  and  if 
the  difference  between  the  current  and  previous  returns  exceeds  a  predefined  threshold.  If 
all  these  conditions  are  met,  the  robot  will  enter  an  outside  turn.  Lines  147-150  set  the 
inTurn  flag,  save  and  reset  the  robot  sigma,  and  set  the  turnType  variable  to  the  value  2 
indicating  the  robot  is  on  a  circular  path.  At  this  point,  currentPath  is  changed  to  a 
circular  arc  (155-163).  The  initial  angle  is  saved  for  later  comparison,  the  kappa  value  is 
calculated,  and  the  steering  feedback  gains  are  set  by  lines  155-161.  Note  here  that  the 

3 

value  used  for  the  radius  is  actually  —  offset  instead  of  offset.  This  was  a  late  change  to 

the  code  but  allows  the  robot  to  make  a  tighter  turn  around  the  comer.  Lines  162, 163  set 
the  configuration  of  currentPath  as  described  by  Equation  (4.8).  The  center  and  radius  of 
currentPath  are  set  in  lines  165-169  as  prescribed  by  Equation  (4.9)  and  its  preceding 
paragraph.  The  next  three  lines  (171-173)  set  the  trackType  to  1  (track  current  path), 
reset  the  sonar  update  field,  and  disable  the  side  sonar. 

If  neither  criteria  (inside  or  outside  turn)  is  met,  the  robot  simply  continues  to 
follow  the  wall  (lines  177,  178). 
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The  brace  on  line  180  closes  the  first  block  of  code.  The  second  block  of  code 
begins  on  line  1 83  and  handles  the  phase  of  motion  when  the  robot  is  in  a  turn  but  not  yet 
in  the  transition  area. 

If  the  robot  is  in  an  inside  turn  ( turnType  —  0)  and  the  robot's  orientation  is 
within  a  predefined  threshold  of  the  current  path  (lines  1 88-190),  then  the  inTransition 
flag  is  set,  the  side  sonar  is  turned  on,  and  the  linear  fitting  data  and  pinged  variables 
are  reset  by  lines  191-194. 

If  not  on  an  inside  turn,  line  197  checks  if  the  robot  is  on  an  outside  turn  (phase  3 
in  Figure  20).  If  so,  then  the  procedure  described  by  Equation  (4.10)  and  its  following 
paragraph  is  performed  by  lines  198-201.  If  the  check  in  line  201  is  passed,  then  the 
robot  is  in  transition  and  lines  202-205  set  the  inTransition  flag,  turn  on  the  sonar,  and 
reset  the  linear  data  and  pinged _ variables. 

When  the  robot  is  on  the  circular  arc  of  an  outside  turn  ( turnType  =  2),  the  first 
condition  of  the  if  statement  on  lines  21 1-215  is  met.  The  next  two  conditions  check  if 
the  robot's  orientation  has  changed  by  (deltaAngle  ±3°)  from  either  the  circular  arc's 
starting  angle  or  the  previous  wall's  angle.  The  angular  error  factor  was  empirically 
selected  through  testing.  If  either  of  these  criteria  is  met,  the  robot  proceeds  straight 
ahead  on  the  straight  portion  of  an  outside  turn  ( turnType  =  1).  Lines  217-224  update 
currentPath  and  turnType  accordingly. 

For  all  three  situations  in  which  the  robot  is  in  a  turn,  the  trackType  flag  is  set  to  1 
(line  229). 
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The  brace  on  line  230  closes  the  second  block  of  code.  The  third  and  final  block 
handles  the  motion  phase  when  the  robot  is  in  the  transition  area  of  a  turn  and  begins  on 
line  233. 

While  in  the  transition  phase,  the  linear  fitting  data  is  being  built.  When  the  data 
becomes  usable,  the  sonar  system  will  set  the  usable  flag.  Line  236  checks  this  flag,  and 
if  the  data  is  usable,  the  robot  will  return  to  the  wall  following  mode  using  the  old  sigma 
value  (lines  237-240).  If  the  data  is  not  yet  usable,  the  robot  remains  in  the  transition 
phase  (line  245). 

The  brace  on  line  247  closes  the  third  motion  phase  block  of  code.  All  that  is  left 
to  do  is  to  calculate  the  commanded  velocities  that  will  be  returned. 

The  switch  statement  on  lines  249-278  calculate  the  steering  function  value  using 
either  the  steerByWall( )  or  steer( )  function.  The  correct  steering  function  is  chosen  by 
the  status  of  the  trackType  variable.  For  the  wall  following  mode  {trackType  =  0),  the 
wall  configuration  is  first  defined.  Lines  252, 253  calculate  the  wall's  x  and  y  coordinates 
using  Equations  (4.1)  and  (4.2).  The  wall's  angle  was  already  computed  so  line  255  uses 
these  values  to  define  curWall.  Lines  259, 260  update  currentPath  using  Equation  (4.4) 
and  line  263  retrieves  the  steering  function  value  using  steerByWall().  Finally,  the  wall's 
current  orientation  is  saved  in  prevWallTheta  (line  266)  to  be  available  for  future  use. 

If  the  trackType  is  1,  the  current  path  will  have  already  computed  and  the  steering 
function  value  is  found  using  the  normal  steer ( )  function  (line  271),  allowing  the  robot  to 
track  the  current  path. 
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After  the  default  case  for  the  switch  statement  (just  a  precaution)  come  the  final 
lines  in  the  function.  Line  280  always  sets  firstTimeCalled  to  zero  since  the  function  is 
only  called  for  the  first  time  once.  Line  282  uses  a  motion  system  function  to  calculate 
the  linear  portion  of  the  commanded  velocities  and  line  284  calculates  the  distance  the 
robot  has  traveled  over  the  last  motion  control  cycle.  This  distance  is  combined  with  the 
steering  function  value,  the  robot's  current  curvature,  and  the  commanded  linear  velocity 
to  compute  the  required  rotational  velocity  (lines  286, 287).  Once  the  commanded 
velocities  have  been  determined,  the  wallHugRule( )  function  is  complete  and  the 
commanded  velocities  are  returned  to  the  calling  routine. 

E.  USER  PROGRAM 

The  previous  sections  described  how  the  wall  following  mode  is  implemented  and 
used  to  set  robot  on  course.  The  user  program,  included  in  Appendix  B,  is  used  for 
operator  interaction  for  setting  up  initial  system  parameters  and  for  end  of  run  condition 
checks.  Before  calling  the  trackWall( )  function,  the  user  program  prompts  the  operator 
for  system  parameters  such  as  whether  sonar  logging  is  on,  the  desired  wall  offset,  the 
sigma  value,  the  linear  decay  factor,  and  which  side  the  wall  will  be  on.  The  program 
uses  this  information  to  initialize  corresponding  parameters  in  the  software  system.  The 
trackWall( )  is  then  used  to  initialize  the  wall  following  mode.  At  this  point  the  robot 
begins  its  motion,  and  the  remaining  portion  of  the  user  program  monitors  the  robot's 
progress  until  end  of  run  conditions  are  met.  Once  the  robot  has  either  halted  or 
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completed  a  complete  circuit,  the  program  shuts  down  the  sonar  system,  stops  the  motion 
processing,  and  returns  to  the  main  program  to  download  logged  data. 
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V.  THESIS  RESULTS  AND  CONCLUSIONS 


A.  RESULTS 

The  goal  of  this  thesis  was  to  give  an  autonomous  vehicle,  such  as  Yamabico,  the 
capability  to  navigate  about  a  room  or  rooms  composed  of  continuously  connected 
orthogonal  wall  segments  while  avoiding  obstacles  in  its  path.  The  result  of  the  research 
effort  in  implementing  this  behavior  produced  the  desired  effect  By  using  the  new 
motion  mode  now  incorporated  in  the  MML  software  library,  Yamabico  can  navigate 
around  such  a  room  in  a  safe,  smooth,  and  efficient  manner.  Several  tests  were  conducted 
to  verify  this  fact  and  the  data  collected  supports  this  conclusion. 

Figure  22  shows  actual  data  collected  from  Yamabico  while  traversing  a  hallway. 
The  x  and  y  axes  are  measured  in  cm.  Both  runs  were  completed  using  the  left  wall 
following  mode.  The  first  run  used  an  offset  of  80  cm  while  the  run  in  the  second  run 
used  a  50  cm  offset.  Both  runs  demonstrate  the  ability  to  maintain  an  offset  distance 
while  traversing  a  straight  wall  and  smoothly  negotiate  both  minor  variations  in  the  wall 
and  concave  comers.  The  angular  tilt  in  the  first  figure  is  caused  by  initial  positioning  of 
the  robot  and  errors  associated  with  dead  reckoning  (DR)  navigation.  Even  though  these 
errors  would  be  significant  in  normal  path  tracking  mode,  they  are  minor  in  the  wall 
following  mode  since  the  robot  is  maintaining  the  appropriate  distance  from  the  reference 
wall,  as  indicated  by  the  wall  data  being  tilted  by  an  equal  amount. 
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X  (cm) 


Figure  22.  Left  Wall  Following  with  80cm  and  50cm  Offsets. 
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To  demonstrate  the  ability  to  negotiate  a  convex  turn  as  well  as  obstacle 
avoidance,  Figure  23  shows  the  motion  and  sonar  data  of  a  run  starting  in  the  comer  of  a 
room  with  convex  comers,  extending  out  and  around  an  outside  hallway  containing  an 
obstacle,  and  then  back  into  the  room  completing  the  circuit.  This  run  was  performed 
using  right  wall  following  with  an  offset  distance  of  50  cm.  The  robot  once  again 
maintains  the  correct  offset  distance  from  straight  wall  segments  while  smoothly 
negotiating  both  concave  and  convex  comers.  As  before,  DR  navigation  errors 
accumulate  during  this  extended  run,  indicated  by  the  increasing  tilt  of  the  walls  as  the 
run  progresses  and  the  difference  between  the  starting  and  finishing  positions.  The  robot, 
however,  maintains  the  correct  offset  distance  from  the  reference  walls  throughout  the 
run.  The  obstacle  is  treated  as  just  another  series  of  turns. 
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Yamabico  Wall  Following  Data  (6/01/99) 


-200  -100  0  100  200  300  400  500  600  700  800  900 

X  (cm) 


Figure  23.  Complete  Circuit  of  Two  Rooms  Using  Right  Wall 
Following  with  50cm  Offset. 
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With  the  successful  implementation  of  a  wall  following  motion  mode,  a  solid 
foundation  has  been  formed  for  the  development  of  many  advanced  abilities. 

B.  FUTURE  WORK 

The  work  completed  in  this  thesis  was  accomplished  with  fairly  broad  goals  in 
mind  and,  although  important,  should  be  considered  as  a  first  step  toward  more  complex 
behaviors.  Some  ideas  for  future  work  are  presented  in  the  following  sections. 

1.  Starting  Point 

During  the  development  of  the  wall  following  mode,  the  robot's  starting  point  was 
always  in  close  proximity  to  the  first  wall.  For  a  more  versatile  behavior,  the  starting 
point  should  not  have  this  limitation.  An  algorithm  could  be  developed  allowing  the 
robot  to  start  anywhere  inside  a  room  and  search  for  the  first  wall.  One  possible 
implementation  might  be  to  have  the  robot  proceed  straight  ahead  until  encountering  a 
wall  and  then  make  a  decision  whether  to  turn  right  or  left  to  enter  the  wall  following 
mode. 

2.  Incorporate  an  S-turn  Ability 

While  the  robot  is  following  a  wall,  minor  variations  in  the  wall  cause  the  robot  to 
adjust  its  course  to  maintain  the  offset  distance.  If  the  robot  senses  a  large  difference  in 
side  sonar  distance  between  the  current  and  previous  readings,  it  enters  a  convex  turn. 
There  is  a  range  of  distances  that  is  less  than  the  threshold  for  convex  turns  but  too  large 
for  the  robot  to  compensate  for.  A  possible  fix  to  this  problem  is  to  incorporate  an  S-tum 
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when  this  situation  arises.  The  robot  could  turn  45°  toward  the  wall  and  then  reverse  this 
turn  to  again  match  the  angle  of  the  wall. 

3.  Other  Than  Orthogonal  Turns 

An  obvious  extension  to  the  work  done  in  this  thesis  would  be  to  develop  a  more 
complex  algorithm  that  would  handle  a  wider  variety  of  turn  angles.  During  the 
development  of  the  straight  wall  following  algorithm,  testing  was  performed  on  walls  that 
varied  in  angle  by  values  up  to  20°.  The  robot  compensated  fairly  well  for  angle 
differences  of  15°  degrees  and  below.  Modifying  the  orthogonal  turn  algorithms  to 
handle  turns  of  30°  should  result  in  a  very  robust  behavior. 

4.  Better  Collision  Detection 

The  current  collision  detection  mechanism  is  rather  elementary.  The  robot  uses  its 
forward  looking  sonar  to  detect  a  situation  where,  by  some  error  or  miscalculation,  it  is 
too  close  to  something  directly  ahead.  This  collision  detection  mechanism  could  be 
improved  by  use  of  additional  sonars,  such  as  those  angled  ±30°  of  the  nose. 

5.  Very  Small  Obstacle  Avoidance 

The  robot  is  capable  of  avoiding  obstacles  in  its  path  as  long  as  those  obstacles  are 
seen  by  the  sensors.  Because  the  forward  looking  sonar  is  positioned  on  the  centerline  of 
the  robot,  small  obstacles  that  are  positioned  inside  the  sonar,  but  in  the  path  of  the  wheel 
closest  to  the  wall,  will  be  hidden  from  the  sensor  and  will  cause  a  collision.  The  solution 
to  this  problem  is  not  simple.  One  idea  would  be  to  use  one  of  the  quartering  sonars  as  a 
warning  detector  and  have  the  robot  make  adjustments  to  avoid  a  collision.  Another  idea 
would  be  to  add  forward  looking  sonars  at  the  comers  of  the  robot.  This  would  very 
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difficult  since  it  would  require  a  hardware  change  and  modification  of  the  software- 
hardware  interface. 
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APPENDIX  A:  MML  FILES  -  MODIFIED  OR  IMPLEMENTATION  RELATED 


/*****★****************************************** ***** ******* ********* *** i 


/*  File:  constants. h  */ 

/*  */ 

/*  last  update  */ 

/*  June  3,  1993  by  Dave  MacPherson  */ 

/*  Wed  04-27-94  Frank  Kelbe  -  update  for  mmlll  */ 

/*  Sun  09-18-94  Cleaned  up  for  clean  irtmlll  version  */ 

/*  May,  1999  Dan  Wells  -  added  constants  for  linear  fit  w/  decay  */ 

/*  */ 


/********************************************************************* *** j 

#ifndef  CONSTANT S_H 

tdefine  _ CONSTANTS_H 

/*  default  motion  logging  filename  */ 
fdefine  MOTIONLOGFILE  "motion.log" 

/*  there  is  no  default  sonar  logging  filename,  because  the  type  of  logging 
must  be  specified  for  sonar  logging,  and  the  default  filename  is  based 
on  the  type  of  logging  to  be  performed  */ 


/*  default  motion  logging  buffer  size  (128K)  */ 
tdefine  MOTIONLOGSIZE  0x20000 

/*  Sonar  logging  buffer  size  (64K)  */ 

#define  SONARLOGSIZE  0x10000 

/*  the  default  10  buffer  size  (64K)  */ 

#define  TRACELOGSIZE  0x10000 


/***  system  limits  ***/ 

/*  max  number  of  sequential  instructions  that  can  go  in  the  buffer  */ 

# define  INST_MAX  500 

/*  max  number  of  tracing/logging  buffers  in  the  system  at  once  */ 

#define  MAXIOBUFFERS  20 

#define  DEFAtJLT_PRECI  S I  ON  6 

tdefine  MAX_REAL_PRECISION  15 

#define  MAX_INTEGER_DI GITS  19 

/*  defines  the  length  of  time  (in  seconds)  of  the  motion  control  cycle  */ 
#define  MOTION_CONTROL  CYCLE  0.01 


/*  generic  defined  constants  */ 


/*  #define  NULL  0  */ 

/*  old  def  of  NULL  above.  Standard  C  def  below  */ 
#ifndef  NULL 

#define  NULL  ((void  *)0) 

#endif 


#define  INFINITY 
# define  INFINITY0 
#define  PI 
#define  DPI 


999999 

99999 

3.14159265358979323846 
6.28318530717958647692  /*  PI  *  2 


*/ 


/*******************************************************  j 
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/★★★★★★★a-**************-****  *****************************y 


/*  Just  info  -  not  used  anywhere  -  right  now  */ 


/***  machine  limits  ** 
#define  DBL_MIN 
# define  DBL_MAX 
#define  INFINITE 
#define  INF 

/*  Numerical  Constants 

#define  HPI 
#define  PI34 
#define  PI4 
#define  RAD 
# define  DAR 


7 

2. 22 5 07 3858507201 4e -308 
1.797  6931348623157e+308 
DBL_MAX 
DBL_MAX 

*/ 

1.57079632679489661923 

2.35619449019234492885 

0.78539816339744830962 

57.29577951308232087684 

0.017453292 


/* 

PI/2 

*/ 

/* 

3PI/4 

*/ 

/* 

PI/4 

*/ 

/* 

180  /  PI 

*/ 

/* 

PI  /  180 

V 

/***★★***★★★*★★★*★********★★****★*★★*★★*★**★★★****★**★******■**********  ^ 
/*  constants  defination  for  track. c  */ 

tdefine  SO  0.0 

tdefine  r2d  57.29577951 

#define  d2r  0.017453292 

#define  bufferMax  100 

#define  QPI  PI/4.0 

/**************★***★*★*************★****************★*****************  j 


/*  constants  for  linear  fitting  with  decay  and  wall  following 

V 

/*  Added  by  Dan  Wells,  May 

99 

*/ 

#define 

DEFAULT  DECAY 

0 . 9f 

#define 

DEFAULT  WALL  OFFSET 

80 

/* 

from  robot  center  */ 

#define 

MIN  EDGE  OFFSET 

10 

/* 

from  robot  edge  */ 

# define 

INSIDE  TURN  SIGMA 

5. Of 

/* 

for  tighter  inside  turns  */ 

#define 

OUTSIDE  TURN  SIGMA 

10. Of 

/* 

for  outside  turns  */ 

#define 

COLLISION  THRESH 

30 

/* 

distance  from  front  sonar  */ 

tdefine 

TRANSITION  ANGLE 

0 . 17453f 

/*  10  degrees  */ 

#define 

OUTSIDE  TURN  THRESH 

80 

/* 

diff  betwn  cur  and  prev  sonar 

retrn  */ 

#endif 

/**★***★*★★★**•*★*★★**★★*★★*★★★**★★★★******★*★★*******★***★**■****.***★★********** 
Author (s):  Scott  Book 

Project:  Yamabico  Robot  Control  System 

Date:  December  8,  1993 

Revised:  May  3,  1994  by  Kevin  Huggins 

May,  1999  by  Dan  Wells  -  wall  following  motion  mode 
File  Name:  definitions .h 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  standard  definitions  and  data  type 

declarations  used  throughout  the  reset  of  the  MML  system. 

******************************************* *********************************** i 


#ifndef  _ DEFINITIONS_H 

#define  _ DEFINITIONS__H 

/*  Always  include  this  because  it  is  needed  by  most  modules  */ 
#include  "constants .h" 

typedef  unsigned  char  BYTE; 
typedef  unsigned  short  WORD; 
typedef  unsigned  long  LONG; 

typedef  enum  {  NOMODE, 

ENDMODE, 

STOPMODE, 
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TRACKMODE, 

TRACKSMODE, 

TRACKRWALLMODE, 

TRACKLWALLMODE, 

ROTATEMODE, 

PARAMODE, 

BIDIRMODE, 

KS  P I RALMODE , 

FOLLOWMODE, 

REGIONMODE, 

PCMODE 
}  MODE; 

typedef  enum  {  NOCLASS, 

LINECLASS, 

CIRCLECLASS, 

BLINECLASS, 

NOSTOPBLINECLASS, 

BIDIRCLASS, 

LEFT, 

RIGHT, 

CENTER, 

CCWLEFT, 

CCWRIGHT, 

CWLEFT, 

CWRIGHT 

}  CLASS; 

typedef  enum  {  INITMOTION, 

INTERMOTION, 
INTRAMOTION, 
FINALMOTION 
}  STAGE; 


typedef  enum  {FALSE  =  0,  TRUE}  BOOLEAN 


typedef  struct  { 
MODE  mode ; 

CLASS  class; 

}  PATH  TYPE; 


typedef  struct  { 
double  Linear; 
double  Rotational; 
}  VELOCITY; 


typedef  struct  { 
double  X; 
double  Y; 

}  POINT; 


typedef  struct { 
POINT  Posit; 
double  Theta; 
double  Kappa; 
}  CONFIGURATION; 


typedef  struct { 

POINT  Focus; 

CONFIGURATION  Directrix; 
}  PARA; 

typedef  struct { 

CONFIGURATION  config; 


PATHJTYPE 
}  PATH_ELEMENT; 


pathType; 


typedef  struct { 

POINT  LeftStart; 

POINT  LeftEnd; 
double  LeftOrient; 

POINT  RightStart? 

POINT  RightEnd; 
double  RightOrient; 

}  REGIONEDGE; 

typedef  struct  Image { 

POINT  coord; 

int  ImageType; 

double  Orient; 

double  ClosedDist; 

}  Image; 

/*  Error  Codes  */ 

typedef  enum  { 

NOERROR, 

ECODEO,  /*  configurations  too  close  */ 

EC0DE1,  /*  erroneous  goal  configuration  orientation  */ 
ECODE2,  /*  SSTOP  function  detected  in  moving  state  */ 
ECODE3  /*  undefined  instruction  class  */ 

}  ERROR_CODE; 

/*ADDEDV 

typedef  struct  { 

MODE 

CONFIGURATION 
double 
double 
double 
double 
POINT 
double 
int 

}  LINE; 

#endif 


Author (s):  Kevin  Huggins 

Project:  Yamabico  Robot  Control  System 

Date:  April  14,  1994 

Revised:  May  3,  1994 

File  Name:  geometry. c 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
Description:  This  file  contains  the  standard  geometry  functions  that  are 
called  by  several  functions. 

******************************************************************************/ 

#include  "definitions .h" 
tinclude  "math.h" 

#include  "geometry. h" 
linclude  "motion. h" 

#include  "stdiosys.h" 
tinclude  "constants .h" 

/★★★a****************************************************** ★***★***★**★***★**★★ 

Function:  eu_dis() 

Purpose:  Computes  the  Eucledian  distance  between  two  given  points 
Parameters:  double  xl,yl,x2,y2 
Returns :  double 


Type; 

config; 

a; 

b; 

c; 

Radius? 

Center? 

speed; 

direction; 
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Comments : 

******************************** ******* ******** ******************************* j 

double 

eu_dis (double  xl,  double  yl, double  x2,  double  y2) 

return  sqrt(((xl  -  x2)  *  (xl  -  x2))  +  ( (yl  -  y2)  *  (yl  -  y2})); 


/******************************■************************************************ 
FUNCTION:  norm() 

PARAMETERS:  double  angle  -  the  angle  to  normalize 

PURPOSE:  normalize  the  input  angle  between  -PI  and  PI 

RETURNS:  double:  the  normalized  angle  in  radians 

COMMENTS:  This  is  the  most  common  normalizing  function  used  in  the  system 

This  performs  that  same  as  norm()  and  normalize) ()  in  MML10. 

****************************************************************************** i 

double 

norm (double  angle) 

/*{ 

while  ((angle  >  PI)  ||  (angle  <=  -PI)) 

{ 

if  (angle  >  PI) 
angle  -=  DPI; 
else 

angle  +=  DPI; 

} 

return  angle; 

}*/ 


return (angle  -  DPI* (ceil ( (angle  +  PI) /DPI)  -  1.0)); 

> 


/★★★★★★★★★★a******************************************************************* 

FUNCTION:  positiveNorm( ) 

PARAMETERS:  double  angle  -  the  angle  to  normalize 

PURPOSE:  normalize  the  input  angle  between  0  and  2PI 

RETURNS:  double:  the  normalized  angle  in  radians 

COMMENTS:  Same  functionality  as  pnorm()  in  MML10 

*************************************** *************************************** ^ 
double 

positiveNorm (double  angle) 

{ 

while  ((angle  >=  DPI)  ||  (angle  <  0)) 

{ 

if  (angle  >=  DPI) 
angle  --  DPI; 
else 

angle  +=  DPI; 

} 

return  angle; 


/************************************************* ***************************** 
FUNCTION :  negativeNorm( ) 

PARAMETERS:  double  angle  -  the  angle  to  normalize 

PURPOSE:  normalize  the  input  angle  between  -2PI  and  0 

RETURNS:  double:  the  normalized  angle  in  radians 

COMMENTS:  Same  functionality  as  nnormO  in  MML10 

double 

negativeNorm ( double  angle) 

{ 

while  ((angle  >  0)  | |  (angle  <=  -DPI)) 

{ 

if  (angle  >  0) 
angle  -=  DPI; 
else 
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angle  +-  DPI; 

} 

return  angle; 

} 

/****************************************************************************** 
FUNCTION:  normPIover2 ( ) 

PARAMETERS:  double  angle  - the  angle  to  normalize 

PURPOSE:  normalize  the  input  angle  between  -PI/2  and  PI/2 

RETURNS:  double:  the  normalized  angle  in  radians 

COMMENTS:  This  was  designed  specifically  for  parabola  calculations 

By  Mahmoud  Wahdan  3/12/96 

******************************************************************************/ 

double 

normPlover2 (double  angle) 

{ 

while  ((angle  >  PI/2)  ||  (angle  <=  -PI/2)) 

{ 

if  (angle  >  PI/2) 
angle  -=  PI; 
else 

angle  +=  PI; 

} 

return  angle; 


/***************************************************************************** 
FUNCTION :  signedDist ( ) 

PARAMETERS:  CONFIGURATIONS  directerix 
POINT  focus 

PURPOSE:  to  calculate  the  size (signed)  distance  between  a  point (focus)  and 

a  configuration  (directrix) . 

RETURNS:  double:  the  signed  difference 

COMMENTS:  This  was  designed  specifically  for  parabola  calculations 

By  Mahmoud  Wahdan  3/12/96 

*******************************************************************************/ 


double 

signedDist (POINT  pt,  CONFIGURATION  config) 
{ 


} 


return  (-(pt.X  -  config . Posit -X)  *  sin (config. Theta)  + 

(pt.Y  -  config . Posit .Y) * 


cos (config. Theta) ) ; 


/****************************************************************************** 
FUNCTION:  def ineConf ig ( ) 

PARAMETERS:  double  x, y, theta, kappa  — The  values  that  define  a 

configuration 

PURPOSE:  To  allocate  nad  assign  a  configuration 

RETURNS:  CONFIGURATION:  a  configuration 

COMMENTS:  Was  called  def_configuration ( )  in  MML10 

************************************************ ******************** ★*★★******/ 
CONFIGURATION 

defineConfig (double  x, double  y, double  theta, double  kappa) 

{ 

CONFIGURATION  newConfig; 

newConfig.Posit.X  =  x; 
newConfig. Posit. Y  =  y; 
newConfig. Theta  =  theta; 
newConfig . Kappa  =  kappa; 

return  newConfig; 

} 


^****************************************************************************** 


FUNCTION: 
PARAMETERS : 

PURPOSE: 

RETURNS: 


def ineParabola ( ) 

double  xf,yf  - defines  the  focus 

double  xd,  yd,  thetad  - defines  the  directrix 

To  allocate  assign  a  parabola 
PARA:  a  parabola  type 


92 


COMMENTS:  Was  called  def jparabola ()  in  MML10 

t******************************************************************,**,*.*****^^^. 

PARA 

defineParabola {double  xf, double  yf,  double  xd, 
double  yd,  double  td) 


PARA  newPara; 

newPara. Focus .X  =  xf; 
ne wPara. Focus. Y  =  yf; 
newPara. Directrix. Posit. X  -  xd; 
newPara. Directrix. Posit .Y  =  yd; 
newPara. Directrix. Theta  =  td; 
newPara. Directrix. Kappa  =  0.0; 
return  newPara; 


/♦★★★a*******************************************  ******************,****,*** ^.***^ 
FUNCTION:  reverseOrientation ( ) 

PARAMETERS:  CONFIGURATION  original  —  the  original  configuration 

orientation  changed  by  180  degrees 
PURPOSE:  To  reverse  the  orientation  of  a  given  configuration 

RETURNS:  CONFIGURATION:  the  reversed  configuration 

COMMENTS:  Was  called  negate ()  in  MML10 

************  ****************************************************.*****^*.**  +  ^^  j 

CONFIGURATION 

reverseOrientation (CONFIGURATION  original) 

CONFIGURATION  reversed; 

reversed. Posit. X  =  original. Posit. X; 
reversed. Posit. Y  =  original. Posit. Y; 
reversed. Theta  =  norm (original. Theta  +  PI); 
reversed. Kappa  =  original .Kappa ; 
return  reversed; 


/*********** ******************************************************************* 
FUNCTION :  f indSymConf ig ( ) 

PARAMETERS:  double  a  —  distance  from  either  point  to  the  intersection  of 

both  lines  determined  by  the  two  configurations 
double  alpha  — The  angular  difference  between  both  orientations 
PURPOSE:  This  function  finds  the  symmetric  configuration  of  a  configuration 

described  by  alpha  and  a  above. 

RETURNS:  CONFIGURATION:  symConfig  — the  symmetic  configuration 

COMMENTS:  Was  called  def_sym()  in  MML10 

One  drawback  to  this  function  is  that  it  is  not  possible  to 
represent  a  symmetric  configuration  whose  alpha  is  equal  to  PI. 
find^symConfigl  ()  is  used  to  cover  these  situations 

************'****+******ic**-k*i'**-i'-k**ie*****le-k*-kic******i'**ic*-irick-****-kit*i'ie1rie-k*'****ic/ 

CONFIGURATION 

findSymConfig (double  a,  double  alpha) 

{ 

^  return  defineConfig (a  *  (1.+  cos (alpha)),  a  *  sin(alpha),  alpha,  0.0); 

/********************************************************.*** ******************* 
FUNCTION :  f indSymConf igl ( ) 

PARAMETERS :  double  d  —  distance  from  the  origin  (base  configuration)  to 

the  symmetric  configuration. 

double  alpha  — The  angular  difference  between  both  orientations 
PURPOSE:  This  finds  the  symmetric  configuration  of  a  configuration 

described  by  alpha  and  a  above. 

RETURNS:  CONFIGURATION:  sym_config  — the  symmetic  configuration 

COMMENTS:  Was  called  def_syml()  in  MML10 

This  function  overcomes  the  drawback  of  the  original 
f ind_sym_config ( )  of  not  being  able  to  handle  the  situation  when 
alpha  equals  PI 

**************************************  ****************************************  ^ 
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CONFIGURATION 

finds ymConfigl (double  d,  double  alpha) 

{ 

double  beta  =  alpha/2; 

return  defineConfig(d  *  cos (beta),  d  *  sin (beta),  alpha,  0.0); 

} 

/******************★****★**********************★**★***★**★**************★*★**** 
FUNCTION:  inverse () 

PARAMETERS:  CONFIGURATION  original  — the  original  configuration 

in  global  coordinates 

PURPOSE:  To  calculate  the  inverse  of  a  given  configuration 

RETURNS:  CONFIGURATION:  the  inversed  configuration 

such  that; 

original  *  inversed  =  Identity 

COMMENTS :  None 

★★★★★★★★★★★★★★★★it*************************************************************/ 

CONFIGURATION 

inverse (CONFIGURATION  original) 

{ 

CONFIGURATION  inversed; 

inversed. Posit. X  =  -original .Posit .X  *  cos (original .Theta)  - 
original. Posit. Y  *  sin (original. Theta) ; 

inversed. Posit. Y  =  original. Posit .X  *  sin (original .Theta)  - 
original. Posit. Y  *  cos (original. Theta ) ; 

inversed. Theta  »  -original .Theta; 
inversed. Kappa  =  -original .Kappa; 

return  inversed; 


/a***************************************************************************** 
FUNCTION :  compose ( ) 

PARAMETERS:  CONFIGURATION  *first  —  pointer  to  the  first  configuration 

♦second  —  pointer  to  the  second  configuration 
PURPOSE:  To  calculate  the  composition  of  the  first  and  second 

configurations 

RETURNS:  CONFIGURATION:  configuration  which  is  the 

composition  of  the  first  and  second  configurations 
COMMENTS:  A  typical  example  of  the  usage  of  this  function  is  to  determine 

the  goal  position  of  a  configuration  in  global  coordinates.  In 
such  an  example,  the  first  argument  would  be  the  original 
configuration  and  the  second  argument  would  be  the  goal 
configuration  in  the  original  configuration's  local  coordinate 
system.  The  resultant  third  argument  would  then  be  the  goal 
configuration  in  global  coordinates .Was  called  comp()  in  MML10 
LAST  UPDATE:  10/25/94  Chien-Liang  Chuang 
******************************************************************************/ 
CONFIGURATION 

compose (CONFIGURATION  *  first,  CONFIGURATION  *  second) 

{ 

CONFIGURATION  third; 
double  x,y,  theta; 
double  xx,yy,tt; 

x  =  second->Posit.X; 
y  =  second->Posit . Y ; 
theta  =  first->Theta; 


xx  =  cos (theta)  *  x  -  sin (theta)  *  y  +  first->Posit .X; 

yy  =  sin (theta)  *  x  +  cos (theta)  *  y  +  f irst->Posit . Y; 

tt  =  first->Theta  +  second->Theta; /*  no  need  to  normalize  10/25/94  Chuang  */ 
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third. Posit .X  =  xx; 
third. Posit .Y  -  yy; 
third. Theta  =  tt; 
third. Kappa  =  first->Kappa; 

return  third; 

} 


/***********  ****  ***  ******  **  *  ***  *  *  ***  *****  ******  **  **  **  ***********  **  **  *********** 


FUNCTION: 

PARAMETERS: 


PURPOSE: 

RETURNS: 
COMMENTS : 


circularArc ( ) 

CONFIGURATION  length  — the  arc  length 

alpha  — the  end  orientation 

config  — pointer  to  the  resultant  configuration 
Given  the  arc  length  and  alpha,  to  calculate  the  final 
configuration 

CONFIGURATION:  pointer  to  the  final  configuration 
The  main  purpose  of  this  function  is  to  be  used  in  conjunction 
with  compose (}  to  form  a  new  next().  In  this  case,  length  would 
actually  be  delta-s  and  alpha  would  be  delta-theta. 

Circular_arc { )  would  determine  the  configuration  after  the  incre¬ 
mental  move  in  the  local  coordinate  system  of  the  original 
configuration.  Then  compose ()  would  take  the  original 
configuration  (in  global  coordinates)  and  the  incremental 
configuration  (in  local  coordinates)  to  determine  the 


incremental  configuration  in  global  coordinates. 
**************************************************  ************************^ 
CONFIGURATION 

circularArc (double  length,  double  alpha) 

{ 


**★  i 


double  alpha2, alpha4; 

alpha2  =  SQR ( alpha ); 
alpha 4  =  SQR(alpha2); 

return  defineConfig( (1-  alpha2/6.0  +  alpha4/120 . 0)  *  length, 

(0.5  -  alpha2/24  +  alpha4/720 . 0)  *  alpha  *  length, 
alpha,  0.0); 


/***************************** ************************************************* 
Function  :  orthogonalize () 

Purpose  :  To  set  the  input  orientation  to  the  nearest  angle  of  a  (integer) 
multiple  of  PI/2  (orthogonal  orientation) 

Parameters:  double  theta 

Returns  :  orthogonal  orientation 

Comments: 

******************************************************************************/ 
static  double 

orthogonalize (double  theta) 

{ 

if  (theta  >=  0.0) 

return  ( ( (int)  ((theta  +  QPI)  /  HPI) )  *  HPI); 
else 

return  (((int)  ((theta  -  QPI)  /  HPI))  *  HPI); 


/****************************************************************************** 
Function  :  computeThetaO 

Purpose  :  To  compute  the  orientation  from  a  point  to  another 

Parameters:  POINT  pi,  POINT  p2 

Returns  :  the  orientation  (type  double) 

Comments: 

******************************************************************************  j 

static  double 

computeTheta (POINT  pi,  POINT  p2) 

{ 
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return  atan2(pl.Y  -  p2.Y,  pl.X  -  p2.X); 

} 


/**********★★*************★★*★**★★★*************★★**★ 

Function  :  areaTriangO 

Purpose  :  To  determine  the  area  of  any  triangle. 

Parameters:  Point  pi  the  first  point 

Point  p2  the  second  point 

Point  p3  the  third  point 

Returns  :  the  area  of  triangle  (type  double) 

Comments  :  July  15,  1996  Mahmoud  Wahdan 
************************  *★***★★■*■*■*•★****★*********★**/ 
static  double 

areaTriang (POINT  pi,  POINT  p2,  POINT  p3) 

{ 

double  area; 

/*  calculate  the  area  of  the  triangle  */ 
area  =  0.5*((p2.X  -  pl.X)  *  (p3.Y  -  pl.Y)  - 
(p3.X  -  pl.X)  *  (p2.Y  -  pl.Y)); 

return  area; 

} 

/*** *************************************  *******************  ******************* 
Function  :  depth ( ) 

Purpose  :  To  compute  the  depth  of  a  point  along  a  line  defined  by  the 
orientation. 

Parameters:  POINT  p,  double  alpha  —  a  point  and  it's  orientation 

Returns  :  the  depth  of  the  point 

Comments: 

*★★*****************★******★**★*★*★**★***********★★*★***★*★**★*★★*************/ 
static  double 

depth (POINT  p,  double  alpha) 

{ 

return  p.X  *  cos (alpha)  +  p.Y  *  sin (alpha); 


/★★a*************************************************************************** 
Function :  Psi_f unction ( ) 

Purpose:  Computes  the  Psi  function  of  two  given  points 
Parameters:  POINT  pl,p2 
Returns :  double 
Comments : 

****★**********★★*★*****************★****************★****★★**★*★*******★*****/ 

double 

Psi (POINT  pi, POINT  p2 ) 


if  {  p2.Y  -  pl.Y  ==  0.0  &&  p2.X  -  pl.X  ==  0.0) 
return  0.0; 
else 

return  atan2(p2.Y  -  pl.Y,  p2.X  -  pl.X); 


/♦★★★★★★★★★★★★★★★★a*********************-**-*******************  ****************** 
Function :  distance ( ) 

Purpose:  Computes  the  distance  between  two  given  points 
Parameters:  POINT  pl,p2 
Returns :  double 
Comments : 

*★**★***★**************************★*******★*****★★*********★***★***★*★*★*****/ 

double 

distance (POINT  pi, POINT  p2) 

{ 

double  X,  Y; 
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X  =  pl.X  -  p2.X; 

Y  =  pi . Y  -  p2.Y; 

return  sqrt (  X*X  +  Y*Y  ) ; 

} 

/**************★*************************************************************** 
Function  :  IntersectLineToLine ( ) 

Purpose  :  To  compute  the  intersection  of  two  lines. 

Parameters:  CONFIGURATION  pi,  CONFIGURATION  p2  —  two  lines 

Returns  :  configuration  of  intersection 

Comments: 

*****************************************************************+****+*******  j 

CONFIGURATION 

intersectLineToLine (CONFIGURATION  pi,  CONFIGURATION  p2) 

{ 

CONFIGURATION  inter; 

inter. Posit. X  =  (  -cos (p2. Theta)  * 

(pi. Posit. X  *  sin (pi. Theta)  -  pi. Posit. Y  *  cos (pi .Theta) ) 

+  cos (pi .Theta)  * 

(p2. Posit. X  *  sin (p2. Theta)  -  p2. Posit. Y  *  cos (p2. Theta ) ) 

) 

/  sin (p2. Theta  -  pl.Theta); 

inter. Posit. Y  =  (  sin (pl.Theta)  * 

(p2. Posit. X  *  sin (p2. Theta)  -  p2. Posit. Y  *  cos (p2 .Theta) ) 

-  sin (p2. Theta)  * 

(pi. Posit. X  *  sin (pi .Theta)  -  pi. Posit. Y  *  cos (pi .Theta) ) 

) 

/  sin (p2. Theta  -  pl.Theta); 

inter. Theta  =  pl.Theta;  /*  in  some  cases,  maybe  p2.t  will  be  needed  */ 
inter. Kappa  =  0.0; 
return( inter) ; 

} 

Function  :  order ( ) 

Purpose  :  To  determine  the  order  (i.e.  CW  or  CCW)  of  three  points 
Parameters:  Point  pi  the  first  point 

Point  p2  the  second  point 

Point  p3  the  third  point 

Returns  :  int  CW  if  clockwise,  CCW  if  counterclockwise 

Comments : 

******************************************************************************  j 

int 

order (POINT  pi,  POINT  p2,  POINT  p3) 

{ 

double  area; 

/*  calculate  the  area  of  the  triangle  */ 
area  «  ((p2.X  -  pl.X)  *  (p3.Y  -  pl.Y)  - 
(p3.X  -  pl.X)  *  (p2.Y  -  pl.Y)); 

if  (area  >  0.0) 
return  (1); 
else  if  (area  <  0.0) 
return  (-1) ; 
else 

return  (0); 
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/*★****★****★*****************  ************************************************** 
File  Name:  motion.c 

Environment:  GCC  ANSI  C  compiler  for  the  SPARC  processor 
Description:  This  file  provides  the  routines  and  data  structures  needed  to 
provide  the  motion  control  capability  for  the  robot.  The  file 
is  divided  into  three  sections  The  first  is  the  private  section 
containong  the  encapsulated  data  and  functions.  The  second 
section  is  the  control  section.  This  section  defines  the 
routines  required  for  motion  control.  The  third  section  is  the 
Immediate  command  section.  This  section  defines  MML * s  immediate 
commands.  The  routines  in  these  last  two  sections  can  be 
accessed  publicly. 

Last  update:  May  30 ,  1995  Midportion  MP  is  added  (modified  July, 20  1995) 
June  18,1995  symmetric  path  tracking  is  added  (testing) 

July  21,1995  SingleregionMP { )  added,  tested  ok. 

July  29,1995  Tearing  up  the  motion.c 

May,  1999  Wall  following  motion  mode  added,  Dan  Wells 
**★******************************★**************★***★***★**★***★**************/ 

/♦♦define  TIMER*/ 

#include  "definitions . h" 

#include  "pcdefs.h" 

/*#include  "PCdefs.h"  */ 

♦include  "wheels. h" 

# include  <math.h> 

♦include  "queue. h" 

♦include  "motionlog.h" 

♦include  "geometry. h" 

♦include  "stdiosys.h" 

♦include  "time.h" 

♦include  "system. h" 

♦include  "trace. h" 

♦include  "motion. c.h" 

♦include  " sonar. h" 

♦include  "sonarmath.h"  /*  Added:  D  Wells  4-12-99  */ 

♦include  "sonarcard.h"  /*  Added:  D  Wells  5-16-99  */ 

♦include  "seqcmd.h" 

♦include  "memsys.h" 


/***★**********★**★********★*******************★********★★******★★************* 
PRIVATE  SECTION 

The  following  section  defines  the  encapsulated  definitions,  data  structures 
and  prototypes  used  in  the  system. 

*******★***★****★****★★**★★***★★**★*★★**★**★★*******★***********★*************/ 

/****★**♦*★★*★*★*★★★★*★★★***★★★**★★*★★*★★★★★*★★*★★★★★★**★★★★* 
constant  definition 

***********★**★*****★********★***********★******★****★*****★/ 

♦define  HALFTREAD  26.20623239064972878310 

♦define  DEFAULT_LIN_ACC  10.0 

♦define  DEFAULT_ROT_ACC  0.25 

♦define  DEFAULT_GOAL_VEL_LIN  30.0 

♦de fine  DEFAULT _GOAL_VEL_ROT  5 . 0 

♦define  SIGMA  20.0 


♦define  LEFT  1 

♦define  RIGHT  0 

♦define  ON  1 

♦define  OFF  0 

♦define  NIL  -1 

♦define  CW  -1 

♦define  COL  0 

♦define  CCW  1 
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/************************************************************ 
global  variables  (in  motion. c  file)  declarations 
**********************★*************************************/ 

BOOLEAN  Halted; 

static  double  deltas; 

static  VELOCITY  Commanded;  ^commanded  velocities  */ 

static  double  kappaCommanded;  /*  commanded  kappa  */ 

static  CONFIGURATION  vehicle;  /*  local  variable  for 

vehicle  configuration*/ 

static  LINE  currentPath;  /*  holds  the  current  path  element  values  */ 

static  VELOCITY  haltedVel; 

static  VELOCITY  goalVel, 

goalAcc; 

static  double  desiredSigma, 

totalDistance; 

static  int  LoopTest; 

static  double  lambdaNew,  lambdaOld; 

static  int  decFlag  =  FALSE; 


/*******★*****  the  following  functions  are  for  model  based  motion  **********/ 

/*********************************************************** 

The  following  static  function  declarations  are  the 
prototypes  for  the  encapsulated  functions. 

************************************************************ i 


/*  calculates  the  next  commanded  linear  velocity  value.  */ 
static  double  computeLinVelocity (double  ActualVelocity, 

double  LastCommandedVelocity) ; 

/*  calles  a  motion  rule  function  based  on  the  mode  of  travel  that  the 
vehicle  is  in  */ 

static  VELOCITY  motionRules (VELOCITY  Actual,  VELOCITY  Commanded); 


/*  calculates  the  distance  remaining  on  a  path  to  reach  a  configuration. 
Used  with  bline  calculation.  */ 
static  double  re stOf Path (CONFIGURATION  ql,  LINE  11); 

/*  determines  whether  the  vehicle  needs  to  decelerate. 

Used  in  bline  calculation  */ 
static  int  needsToDecelerate (double  actualVelocity) ; 


/*  determines  whether  it's  time  to  process  the  next  instruction  */ 


void 

setSteeringVar ( ) ; 

/******  Global  variables  for  parabola  tracking  ******/ 
Static  VELOCITY 

stopRule (VELOCITY  actual,  VELOCITY  commanded) ; 
static  VELOCITY 

trackRule (VELOCITY  actual,  VELOCITY  commanded); 
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Static  VELOCITY 

wallHugRule (VELOCITY  actual,  VELOCITY  commanded) ; 
static  VELOCITY 

rotateRule (VELOCITY  actual,  VELOCITY  commanded); 

double  steer (CONFIGURATION  robot,  LINE  line); 
double 

steerByWall (CONFIGURATION  robot,  CONFIGURATION  curWall,  double  offset); 


/*★★*******★**★**************★***********★**********★**★*********************** 


MOTION  CONTROL  SECTION 

The  following  section  defines  the  functions  that  provide  access  to  the 
motion  control  system.  These  routines  are  public. 

******************************************************************************  j 

/****************************************************************************** 
Function  InitMotionO  initializes  all  of  the  private  global  variables 
in  this  module  to  the  default  values.  It  then  calls  SetTimer  to 
program  the  5th  timer  on  serial  board  #l(the  second  serial  board) to  generate 
synchronous  interrupts  every  10ms.  After  the  timer  has  been  set  up,  the 
interrupt  handling  routine  is  made  available  to  the  system  by  the  call  to 
SetMotionlnterruptHandler ( ) . 

★  ★★**★******★★★*★***★★+★★****★**★*********★**★********■*■★*******★******■*.*■*•*  +  •*•*•*  j 


void 

InitMotion (void) 

{ 

LoopTest  =  0; 

Halted  =  FALSE; 
haltedVel. Linear  =  0.0; 
haltedvel .Rotational  =  0.0; 
decFlag  -  FALSE; 

/*  Initialize  motion  related  systems  */ 

InitSeqcmd  ()  ; 

InitWheels  ( ) ; 

/*  Initializes  the  distance.  Updated  every  motion  control  cycle  by  deltas*/ 
setTotalDistancelmm(O.O) ; 

/*  Initialize  the  goal  velocities  */ 
setLinVel Imm ( DE FAULT_GOAL__VE L_L I N )  ; 
setRotVellmm  (DEFAULT_GOAL__VEL_ROT)  ; 

/*  Initialize  the  commanded  velocities  */ 

Commanded. Linear  =  0.0; 

Commanded. Rotational  =  0.0; 

/*  Initalize  the  linear  and  rotational  acceleration  */ 
setLinAccImm  ( DEFAULT__LIN_ACC )  ; 
setRotAccImm(DEFAULT_ROT_ACC)  ; 

/*  Initialize  the  size  constant  */ 
setSigmalmm (SIGMA)  ; 


/*  add  a  part  of  trackline  */ 

/*  Initialize  the  commanded  kappa  */ 
kappaCommanded  =  0.0; 

/*  Initialize  the  clearance  */ 
/*setClearanceImm (CLEARANCE) ; */ 

/*  Initialize  the  vehicle  configuration  */ 
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vehicle. Posit. X  =  0.0; 
vehicle . Posit .Y  =  0.0; 
vehicle. Theta  **  0.0; 
vehicle. Kappa  =  0.0; 

/*  Initalize  the  current  path  configuration  */ 
current Path. Type  =  STOPMODE; 
lambda Old  =  INF; 


/★■AT************************************************************************.**** 

Function  MotionSysControl ( )  is  the  interrupt  handler  workhorse.lt  is  called 
from  the  assembly  interrupt  handler  shell.  Its  first  task  is  to  update  the 
change  in  position  and  orientation  through  calls  to  the  module  responsible 
for  movement.  It  then  uses  this  information  in  the  motion  control  laws  to 
derive  the  commanded  linear  and  rotational  velocities  required  for  this 
motion  control  cycle.  Finally,  it  passes  these  computed  velocities  back  to 
the  movement  module  for  execution. 

*******************************************************************  j 


void 

MotionSysControl (void) 

{ 

double  deltaTheta; 

VELOCITY  Actual;  /*  variable  used  to  hold  the  actual  vehicle  velocity  */ 

#ifdef  TIMER 

int  tick  =  getCount ( ) ; 

#endif 

UpdateMovementO ;  /*  updates  the  distance  traveled  by  both  wheels — found  in 
Wheels. c  */ 

deltas  =  GetDistanceTraveled (currentPath) ;  /*returns  the  linear  distance  the 
vehicle 

has  travelled  between  the  last  two  calls 
to 

UpdateMovementO — found  in  Wheels. c  */ 

/*  updated  by  mahmoud  wahdan  Nov,  14  95 


deltaTheta  =  GetOrientationChange ( ) ;  /*  returns  the  difference  between  the 

changes  in  the  distance  of  the  left 
and  right  wheels  between  the 
last  two  calls  to  UpdateMovement ( ) . 
Found  in  Wheels. c  */ 

totalDistance  +=  deltas;  /*  Keeps  track  of  the  total  distance  traved 

by  vehicle  */ 

/*update  the  vehicle's  configuration  based  on  the  distance  travelled 
during  the  last  motion  control  cycle  */; 

vehicle  =  localize (vehicle,  deltas,  deltaTheta); 


/*  next  2  lines  calculate  the  actual  velocity  that  robot  maintained  based 
on  the  distance  travelled  over  the  last  motion  control  cycle.  */ 

Actual. Linear  =  deltas  /  MOTION_CONTROL__CYCLE; 

Actual. Rotational  =  deltaTheta  /  MOTION_CONTROL_CYCLE; 

/*  logs  the  values  of  the  vehicle  configuration.  Data  is 
written  to  a  buffer  during  each  motion  control  cycle  and 
then  downloaded  to  a  file  when  the  program  ends.  LogMotion 
is  found  in  motionlog.c  */ 


LogMotion (vehicle) ; 


/*  motionRules  returns  the  commanded  velocities  that  will  be 

used  in  the  next  motion  control  cycle.  Found  in  this  module.*/ 
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Commanded  =  motionRules (Actual, Commanded) ; 

/*  SetMovement ( )  translates  the  commanded  linear  and 

rotational  velocities  into  commanded  velocities  for  each 
wheel.  Found  in  wheels. c  */ 

SetMovement  (Commanded. Linear, Commanded. Rotational)  ; 

/♦transition () ;  reads  next  instruction  if  needed.  Found  in  this  module.*/ 

/*  Increments  the  "time”  every  motion  control  cycle  for  the 
various  timer  functions.  Found  in  time.c  */ 

clockTick ( ) ; 

/*  blinkLED  is  used  to  control  output  from  interrupt  driven 
motion  control  system.  It  turns  an  LED  on  and  off  every 
second.  Function  found  in  this  module.*/ 

blinkLED () ; 

} 


/**********★******★*****************★★******★***************★★****************★ 
Function  :  setRobotConfigImm( ) 

Purpose  :  To  set  and  update  the  robot  configuration 
Parameters:  CONFIGURATION  NewConfig 
Returns  :  void 
Comments  : 

I******************************************************************************  j 

void 

setRobotConfiglmm (CONFIGURATION  NewConfig) 

{ 

Disablelnterrupts ( ) ; 

vehicle .Posit .X  =  NewConfig. Posit .X; 
vehicle. Posit. Y  =  NewConfig . Posit . Y ; 
vehicle. Theta  =  NewConf ig. Theta ; 
vehicle. Kappa  =  NewConfig. Kappa; 

Enablelnterrupts ( ) ; 

} 


/****************************************************************************** 
Function  :  getRobotConf ig ( ) 

Purpose  :  Retreives  the  current  robot  configuration 

Parameters:  Pointer  to  a  variable  where  the  current  values  for  the  robot's 
configuration  will  be  placed. 

Returns  :  void 
Comments : 

*********★********★★**★******★★★  *  ++*'**+*****  +  ■*++*+***■*+■*•*******  +  *******+*****'1'/ 

CONFIGURATION 

getRobotConfig (void) 

{ 

CONFIGURATION  temp; 

Disablelnterrupts ( ) ; 
temp  =  vehicle; 

Enablelnterrupts () ; 
return  temp; 


/a*************************************************************************** 
Function  :  localize () 

Purpose  :  Calculates  the  next  configuration  of  the  vehicle  based  on  the 
distance  that  the  robot  travelled  during  the  last  motion 
control  cycle 
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Parameters: 


Returns 
Comments : 


CONFIGURATION  robot  — from  the  last  motion  control  cycle 
double  deltas  —  linear  distance  travelled  in  last 
motion  control  cycle 

double  deltaTheta  — angular  change  in  the  last  motion 
control  cycle 

CONFI FURAT I ON  — of  the  vehicle  based  on  the  distance  travlled 
during  the  last  motion  control  cycle 


*************************************************************************** ^ 


CONFIGURATION 


localize (CONFIGURATION  robot,  double  deltas,  double  deltaTheta) 

{ 

CONFIGURATION  tempRobot; 


tempRobot  =  circularArc (deltas,  deltaTheta); 
robot  =  compose (Srobot,  StempRobot); 
robot. Kappa  -  kappaCommanded; 

return  robot; 

} 


/***************************************************************************** 
Function  :  computeLinVelocity ( ) 

Purpose  :  Calculates  the  linear  component  of  the'  commanded  velocity. 
Parameters:  double  ActualVelocity,  double  CommandedVelocity 
Returns  :  new  commanded  linear  velocity 
Comments : 

*****************************************************************************y 

static  double 

computeLinVelocity (double  ActualVelocity,  double  CommandedVelocity) 

{ 

double  VelocityChange; 

double  goalSpeed; 

VelocityChange  *  goalAcc. Linear  *  MOTION__CONTROL_CYCLE; 

goalSpeed  =  goalVel. Linear  /  (1.0  +  HALFTREAD  *  fabs (kappaCommanded) ) ; 

if  (ActualVelocity  <  goalSpeed) 

CommandedVelocity  =  Min (CommandedVelocity  +  VelocityChange, 
goalSpeed) ; 

else 

CommandedVelocity  -  Max (CommandedVelocity  -  VelocityChange, 
goalSpeed) ; 


return  CommandedVelocity; 

} 

/***************************************************************************** 
Function  :  computeLinVelocityS ( ) 

Purpose  :  Calculates  the  linear  component  of  the  commanded  velocity. 
Parameters:  double  ActualVelocity,  double  CommandedVelocity 
Returns  :  new  commanded  linear  velocity 
Comments : 

***************************************************************************** ^ 
static  double 

computeLinVelocityS (double  ActualVelocity,  double  CommandedVelocity) 

{ 

double  VelocityChange; 

double  goalSpeed; 

VelocityChange  =  goalAcc. Linear  *  MOTION_CONTROL_CYCLE; 

if(decFlag  | I  needsToDecelerate (ActualVelocity) ) 

{ 

CommandedVelocity  VelocityChange; 
if (CommandedVelocity  <=  0.0) 
currentPath.Type  =  STOPMODE; 

} 
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else 

{ 

goalSpeed  *  goal Vel . Linear  /  (1.0  +  HALFTREAD  *  fabs (kappaCommanded) ) ; 


} 


if  (ActualVelocity  <  goalSpeed) 

CommandedVelocity  =  Min (CommandedVelocity  +  VelocityChange, 
goalSpeed) ; 

else 

CommandedVelocity  =  Max (CommandedVelocity  -  VelocityChange, 
goalSpeed) ; 


} 

return  CommandedVelocity; 


/***★*******★★*★***********★*★********★**************************************** 
Function  :  restOfPath() 

Purpose  :  calculates  the  remaining  distance  to  the  ending  configuration  for 
Parameters:  void 

Returns  :  the  distance  between  current  position  to  the  STOPPING 

configuration 

Comments : 

***************************** *************************************************^ 
static  double 

restOfPath (CONFIGURATION  ql,  LINE  11) 

{ 

CONFIGURATION  q2  =  ll.config; 
if  (q2. Kappa  ==  0.0) 

return  ((q2. Posit. X  -  ql. Posit. X)  *  cos (q2 .Theta)  + 

(q2. Posit. Y  -  ql. Posit. Y)  *  sin (q2 .Theta) ) ; 

else 

if  (q2. Kappa  >  0.0) 
return  (11. Radius* 

positiveNorm(Psi (11. Center ,q2. Posit)  - 
Psi (11. Center, ql. Posit) ) ) ; 

else 

return  (11. Radius* 

negativeNorm{Psi (11. Center, q2. Posit)  - 
Psi (11. Center, ql. Posit) ) ) ; 


/***************************************** ************************************* 
Function  :  needToDecelerate ( ) 

Purpose  :  To  determine  whether  the  robot  needs  to  begin  decelerating.  Such 
as  in  a  bline  function. 

Parameters:  double  actualVelocity  (linear) 

Returns  :  It  returns  1  if  it  needs  to  decelerate.  Otherwise,  it  returns  0. 
Comments : 

*******************************************************************************y 

static  int 

needsToDecelerate (double  actualVelocity) 

{ 

int  decelerate; 
decelerate  -  FALSE; 

if  (2.0  *  goalAcc. Linear  *  restOfPath (vehicle, currentPath) 

<-  actualVelocity  *  actualVelocity) 
decelerate  =  TRUE; 

decFlag  =  decelerate; 
return  decelerate; 


/****************★*******★********************************.********************* 
Function  :  motionRules ( ) 

Purpose  :  To  calculate  the  linear  velocity  and  rotational  velocity  based 
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on  the  type  of  motion  that  the  robot  is  executing. 

Parameters:  VELOCITY  actual,  commanded 

Returns  :  The  commanded  linear  and  rotational  velocities. 

Comments : 

******************* ****** **************************** ****** ******* ************ j 

static  VELOCITY 

motionRules (VELOCITY  actual,  VELOCITY  commanded) 

{ 

switch  (currentPath.Type)  { 
case  STOPMODE: 

commanded  -  stopRule (actual,  commanded); 
break; 

case  TRACKMODE: 
case  TRACKSMODE: 

commanded  =  trackRule (actual,  commanded); 
break; 

case  TRACKRWALLMODE: 
case  TRACKLWALLMODE : 

commanded  =  wallHugRule (actual,  commanded); 
break; 

case  ROTATEMODE: 

commanded  =  rot ateRule (actual,  commanded); 
break; 

default: 

break; 

} 

return  commanded; 

> 

/****************************************************************************** 
Function  :  stopRule ( ) 

Purpose  :  updates  the  commanded  velocity  to  0  (zero)  to  stop  the  robot 
Parameters:  VELOCITY  actual,  commanded 

Returns  :  The  required  linear  velocity,  rotational  velocity 
Comments : 

***********************************  *********************  *-***■*.*.*  ***************  j 

static  VELOCITY 

stopRule (VELOCITY  actual,  VELOCITY  commanded) 

{ 

/*if  there  is  a  next  line  in  the  buffer,  take  out  as  the  current  line*/ 

commanded. Linear  =  0.0; 
commanded. Rotational  =  0.0; 

if ( ! IsBuf f erEmpty ( ) ) 

{ 

currentPath  =  GetLineO; 

} 

return  commanded; 

} 

/********************************************************************** ******** 
Function  :  testNeutral ( ) 

Purpose  :  to  determine  if  the  robot  is  at  the  neutral  switching  point 
Parameters:  None 
Returns  : 

Comments  : 

************** ********************************************** ****************** i 

int  testNeutral (LINE  nextLine,  double  lambdaNew  ) 

{ 

double  angle; 

if  (nextLine. config. Kappa  ==  0)  { 
if  (cur rent Path. con fig. Kappa  0)  { 

angle  =  norm (nextLine. config. Theta  -  currentPath. config. Theta ) ; 
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if  {fabs (angle)  <  0.001)  { 
return  TRUE? 

}  else  { 

return  (angle* lambdaNew  >=  0.0); 

} 

}  else  { 
return  0; 

} 

}  else  { 
return  0; 

} 

} 

/★★a*********************************************************** **************** 

Function  :  trackRule() 

Purpose  :  To  determine  the  linear  and  rotational  velocities  needed  to  put  or 
keep  Yamabico  on  the  path. 

Parameters:  VELOCITY  actual ,  commanded 

Returns  :  The  required  linear  velocity,  rotational  velocity 
Comments : 

★★★★★★a*********************************************************************** i 

static  VELOCITY 

trackRule (VELOCITY  actual,  VELOCITY  commanded) 

{ 

double  deltaDistance,  angle; 

LINE  nextLine? 

if ( ! IsBuf ferEmpty ( ) ) 

{ 

nextLine  =  ReadNextLine ( ) ; 

lambdaNew  =  steer (vehicle,  nextLine) ? 

angle  =  fabs (norm (Psi (vehicle .Posit, nextLine .Center) 

-  vehicle .Theta) ) ; 

if  ( (lambdaOld  !=  INF)  &&  ( lambdaNew* lambdaOld  <=  0.0) 

&&  ( (currentPath.config. Kappa  --  0.0) 
j  j  (nextLine. config. Kappa  ==  0.0) 

||  (distance (vehicle . Posit,  nextLine .Center)  <= 
fabs (nextLine. Radius)  *  0.9) 

|  |  (angle  <  HPI*0.9))) 

/*  if  (testNeutral (nextLine,  lambdaNew))*/ 

{ 

currentPath  =  GetLine ( ) ; 
lambdaOld  =  INF? 

} 

else 

{ 

lambdaOld  =  lambdaNew; 

} 

} 

if  (currentPath. Type  ==  TRACKMODE) 

commanded. Linear  =  computeLinVelocity (actual .Linear, commanded. Linear) ; 
else 

commanded. Linear  =  computeLinVelocityS  (actual. Linear, commanded. Linear) ; 

deltaDistance  =  MOTION_CONTROL_CYCLE  *  commanded. Linear; 

kappaCommanded  =  vehicle .Kappa  + 

steer (vehicle, currentPath)  *  deltaDistance; 
commanded. Rotational  =  kappaCommanded  *  commanded. Linear; 

return  commanded; 

} 

/a**********************************************************.*.*.*******.********* 
Function  :  wallHugRule ( ) 

Purpose  :  To  determine  the  linear  and  rotational  velocities  needed  to  put 
or  keep  Yamabico  at  the  correct  offset  from  the  wall. 

Parameters:  VELOCITY  actual,  commanded 
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Returns  :  The  required  linear  velocity,  rotational  velocity 
Comments:  Added  by  Dan  Wells  for  wall  following.  May  99. 

*************************************************************************★★**/ 

/***********************************************  j 

/****  code  LISTED  IN  APPENDIX  C  OF  THESIS  ****/ 
/★************★**********★**********************/ 

/★I*****************************************************************************/ 

double  OldLambdaO 

{ 

return  lambdaOld; 

} 

double  NewLambdaO 

{ 

return  lambdaNew; 

} 

/********************** **************** **************************************** 
Function  :  rotateRule ( ) 

Purpose  :  updates  the  commanded  velocity  to  rotate  the  robot 
Parameters:  VELOCITY  actual,  commanded 

Returns  :  The  required  linear  velocity,  rotational  velocity 
Comments:  last  updated  10/25/94  Chien-Liang  Chuang 

*********★★***************★********************★******************************/ 
static  VELOCITY 

rotateRule (VELOCITY  actual,  VELOCITY  commanded) 

{ 

static  double  goalTheta; 
static  int  noGoalTheta  =  TRUE? 

double  deltaVel  =  goalAcc. Rotational  *  MOTION_CONTROL_CYCLE; 
double  rotVel? 
double  rotAcc; 
int  Direction; 

rotVel  -  goalVel. Rotational; 
rotAcc  =  goalAcc. Rotational; 
if  (noGoalTheta) 

{  /*  set  the  goal  of  rotation  */ 

goalTheta  -  vehicle .Theta  +  currentPath.config. Theta; 
noGoalTheta  =  FALSE; 

} 

commanded . Linear  =  0.0; 

Direction  =  (currentPath.config. Theta  >=0.0)  ?  CCW  :  CW; 

/*  First,  handle  the  acceleration  */ 
if  (2.0  *  rotAcc  *  fabs (goalTheta  -  vehicle. Theta)  > 
commanded . Rotational  *  commanded. Rotational) 

{ 

if  (Direction  ==  CCW)  /*  for  CCW  rotation  */ 

commanded. Rotational  =  Min (commanded. Rotational  +  deltaVel,  rotVel); 
else  /*  for  clockwise  rotation  */ 

commanded. Rotational  =  Max (commanded. Rotational  -  deltaVel,  -rotVel); 

} 

else 

{  /*  Now,  handle  the  deceleration  */ 

if  (Direction  ==  CCW)  /*  for  CCW  rotation  */ 

{ 

if  (vehicle. Theta  <  goalTheta) 

commanded. Rotational  =  Max (commanded. Rotational  -  deltaVel,  0.0); 
else 
{ 

LEDon ( 4 ) ;  /*  turn  on  the  LED  #4  when  finish  the  rotation  */ 
cur rent Path. Type  =  STOPMODE; 
commanded. Rotational  =  0.0; 
noGoalTheta  =  TRUE; 

} 

} 

else 
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{  /*  for  CW  rotation  */ 

if  (vehicle. Theta  >  goalTheta) 

commanded. Rotational  -  Min (commanded. Rotational  +  deltaVel,  0.0); 
else 
{ 

LEDon(4);  /*  turn  on  the  LED  #4  when  finish  the  rotation  */ 
currentPath.Type  =  STOPMODE; 
commanded. Rotational  =  0.0; 
noGoalTheta  -  TRUE; 

} 

} 

> 

return  commanded; 

} 


/***************★******** ***************************** ************************* 
Function:  setLinVellmmO 

Purpose:  sets  and  updates  the  linear  velocity  of  the  robot 
Parameters:  double  LinearVelocity 
Returns :  void 
Comments : 

********* ****************************************************** **************^ 
void 

setLinVellmm (double  linearVelocity) 

{ 

goalVel. Linear  *  linearVelocity; 

} 


/********★************************************ ********************************* 
Function:  setRoVellmmO 

Purpose:  Sets  and  updates  the  rotational  velocity 
Parameters:  double  RotationalVelocity 
Returns :  void 
Comments : 

******************************* *********************************************** i 

void 

setRotVelImm( double  RotationalVelocity) 
goalVel. Rotational  =  RotationalVelocity; 


/******★*********************************************************************** 
Function:  setLinAccImmO 

Purpose:  Sets  and  updates  the  linear  acceleration 

Parameters:  double  LinearAcceleration 
Returns :  void 
Comments : 

************************************** ******** ********************** ********** i 

void 

setLinAccImm (double  LinearAcceleration) 

{ 

goalAcc. Linear  =  LinearAcceleration; 

} 


/***********************************************************^.^.*  +  ^.^.+  +  A^^.^^^A^.^  +  + 
Function :  setRotAccImm ( ) 

Purpose:  Sets  and  updates  the  rotational  acceleration 
Parameters:  double  RotationalAcceleration 
Returns:  void 
Comments : 

******************************************************************<A,  +  ^.+^^,i^<i^.^,+  +  y 

void 

setRotAccImm (double  RotationalAcceleration) 
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goalAcc. Rotational  -  Rot ationalAccele ration; 

} 

/****************************************************************************** 
Function:  setTotalDistancelmmO 

Purpose:  sets  the  total  distance  travelled  by  the  robot  to  the  value  passed 
as  a  parameter 
Parameters:  double  distance 
Returns :  void 
Comments : 

******************************************************************************/ 

void 

setTotalDi stance Imm ( double  distance ) 

{ 

totalDistance  =  distance; 


/****★***★*★★ ************************** ************************** ************** 
Function:  getTotalDistancelmmO 

Purpose:  returns  the  total  distance  travelled  by  the  robot 

Parameters:  void 

Returns:  double  totalDistance 

Comments : 

****************************************************************************** i 

double 

getTotalDistancelmm ( void) 

{ 

return  totalDistance; 

> 


y****************************************************************************** 
Function :  haltMotionlmm ( ) 

Purpose:  brings  the  robot  to  a  rest.  Is  different  from  stop  in  that  it's 
original  velocity  is  saved  to  be  later  used  by  the  resume 
command  to  allow  the  robot  to  continue  travelling  at  the  same 
speed. 

Parameters:  void 
Returns:  void 
Comments: 

****************************************************************************** ^ 
void 

haltMotionlmm (void) 

{ 

if  ( ! Halted)  { 

Halted  =  TRUE; 

haltedVel. Linear  =  goalVel. Linear; 
haltedVel. Rotational  =  goalVel. Rotational; 

WheelsDisable ( ) ; 

} 

} 

/****************************************************************************** 
Function :  resumeMotionlmm { ) 

Purpose:  Allows  the  robot  to  resume  the  speed  it  was  travelling  before  the 
haltMotionlmm ()  command  was  given. 

Parameters:  void 
Returns:  void 
Comments : 

****************************************************************************** i 

void 

resumeMotionlmm (void) 

{ 

if  (Halted)  { 

Halted  =  FALSE; 

goalVel. Linear  -  haltedVel. Linear; 
goalVel. Rotational  =  haltedVel .Rotational; 
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} 


WheelsEnable ( ) ; 


} 


/★★★★★★★★★★★★★★★a*********************************************************** 

Function  :  blinkLED ( ) 

Purpose  :  To  control  the  output  from  the  interrupt  driven  motion  control 
system.  LoopTest  is  assigned  zero  every  second. 

Parameters:  void 
Returns  :  none 
Comments : 

*************************************************************************** y 

void 

blinkLED (void) 

{ 

if  (LoopTest++  >=  (  (int)  ( <1.0 /MOTION JOONTROL_CYCLE)  -  1)))  { 

changeLEDstate ( 7 ) ; 

LoopTest  =  0; 

) 


^***********************************************-****************<*-*******'****'*** 
Function:  stopImmO 

Purpose:  updates  the  goal  velocity  to  zero  inorder  to  stop  the  robot 
Parameters:  void 
Returns :  void 

Comments:  This  is  the  immediate  stop  command 

*★*★★***★**★***★** **************************************************.**********/ 
void 

stoplmm(void) 

{ 


WheelsDisable ( ) ; 
goalVel. Linear  =  0.0; 
goalVel. Rotational  «  0.0; 

cur rentPath. Type  «  STOPMODE;  /^update  the  path  element  in  motion. c  */ 


} 

/***★**★***★**************★***★**★****★★*****★★*★★***************★************•* 
Function  :  waitMotionEnd ( ) 

Purpose  :  To  wait  until  the  motions  specified  in  user  program  end. 
Parameters:  none 
Returns  :  void 

Comments:  If  the  conditions  are  not  satisfied,  then  it  stays  in  busy  waiting 

****+***+++•*■**■*++***********•*■**■**•*•*■****+•**+***++*■*•*+****++•****  +  ****  +  **++******/ 
void 

waitMotionEnd ( ) 

{ 

while  ( ! (IsBufferEmptyO  &&  (currentPath.Type  =-  STOPMODE))); 

} 

/*★*****★********★*************************************************.*.****.******* 
Function:  setSigmalmmO 

Purpose:  sets  the  size  constant  which  influences  the  sensitivity  of  the 
steering  function 
Parameters:  double  sigmaln 
Returns :  void 

Comments:  last  updated  10/25/94  Chien-Liang  Chuang 

******************************************************************************  j 

void 

setSigmalmm (double  sigmaln) 

{ 

desiredSigma  -  sigmaln; 
return; 


no 


/************************************************** **************************** 
Function :  setSigmalmm ( ) 

Purpose:  returns  the  current  value  of  desiredSigma 
Parameters:  void 
Returns :  double 
Comments : 

*********************************************************** ****************+^^ 
double 

sigma Value () 

{ 

return  desiredSigma; 

} 


/******************************************** ******** ************************** 
Function  :  setPathElement < ) 

Purpose  :  To  set  the  current  path  for  the  robot 
Parameters:  LINE  newPath 
Returns  :  void 

Comments  :  function  added  by  D.  Wells  5/5/99  for  wall  following  motion  mode 

************************************************************************ *^***+ i 

void 

setPathElement (LINE  newPath) 

{ 

current Path  *  newPath; 

} 

/* ADDED  used  by  wheels. c*/ 

/************ ***************************************************************** 
Function  :  getPathElement ( ) 

Purpose  :  retrieves  the  current  path  element  in  the  motion  control  module 
Parameters:  void 
Returns  :  PATH_ELEMENT 
Comments  : 

*********************************** ******************************* ***********^ 
LINE 

getPathElement (void) 

{ 

return  currentPath; 

> 


double  VelocityLinear () 

{ 

return  Commanded. Linear; 
} 


/******************************************************************** 
FUNCTION  :  steer (robot, line)  PURPOSE  :  evaluate  steering 
function 

************************************************** ******************* j 

double  steer (CONFIGURATION  robot,  LINE  line) 


double  lambda,  angle,  dist; 

if  ( line. config. Kappa  ==0.0) 

lambda  =  -  line. a  *  robot. Kappa 

-  line.b  *  norm (robot. Theta  -  line. config. Theta) 

-  line.c  *(- (robot. Posit. X  -  line. config. Posit .X)  *  sin (line. config. Theta) 

+( robot. Posit. Y  -  line. config. Posit .Y)  * 

cos (line. config. Theta) ) ; 
else 
{ 

angle  =  Psi (robot. Posit,  line. Center) ; 
dist  =  distance (line. Center,  robot. Posit) ; 
if  (line. config. Kappa  >0.0) 


in 


{ 

lambda  =  -  line. a  *  (robot. Kappa-line. config. Kappa) 

-  line.b  *  norm (robot. Theta- (angle-HPI) ) 

-  line.c  *  (line. Radius  -  dist) ; 

> 

else 

lambda  =  -  line. a  *  (robot .Kappa-line. config. Kappa) 

-  line.b  *  norm(robot .Theta- (angle+HPI) ) 

-  line.c  *  (line. Radius  +  dist); 

} 

return  lambda; 


/*  This  function  is  grabbing  the  currentPath  before  the  turn. 
We  will  make  more  tests  later.*/ 

double  waitOnTrack (CONFIGURATION  path) 

{ 

double  lambda  =  20.0; 

CONFIGURATION  robot 1 ; 

double  kk; 
double  k,  kk2; 

LINE  pathElement; 

double  sigma  «  sigmaValue {) ; 

pathElement .config  =  path; 

kk  -  path. Kappa  ; 

kk2  =  kk  *  kk; 

k  =  1.0/sigma  ; 

pathElement . a  *  3.0*k  ; 

pathElement .b  =  3.0*k*k  -  kk2  ; 

pathElement. c  =  k*k*k  -  3.0*k*kk2  ; 

do  { 

robotl  =  getRobotConf ig ( ) ; 

lambda  =  fabs (pathElement .a  *  robotl .Kappa) 

+  fabs (pathElement. b  * 

norm (robotl .Theta  -  pathElement .config. Theta ) ) 

+  fabs (pathElement. c  * 

(- (robotl. Posit. X  -  pathElement .config. Posit .X) 

*  sin (pathElement. config. Theta) 

+  (robotl. Posit. Y  -  pathElement .config. Posit .Y) 

*  cos (pathElement .config. Theta) ) ) ; 

}  while (lambda  >  0.05); 

return  lambda; 


/****★★****★******★*★★*★******************★**★★*★*★★*****★★******************** 
Function:  steerByWall (CONFIGURATION,  CONFIGURATION,  double) 

Purpose:  Computes  and  returns  the  steering  function  value  for  the 
linear  fitted  (with  decay)  wall  segment 
tracking  based  on  the  current  state  of  the  robot 
Parameters:  CONFIGURATION  robot  —  current  robot  config 

CONFIGURATION  curWall  —  current  linear  fitted  data  for  wall 
double  offset  —  the  amount  of  desired  offset  from  the  wall 
(+  for  right  wall,  -  for  left  wall) 

Returns:  double  —  The  wall  following  steering  function's  value 
Comments:  Added  by  D  Wells  4-12-99 

★*************★★****★**★*****★**★****★**★*******★*★*★★★********★***★**********/ 

double 

steerByWall (CONFIGURATION  robot,  CONFIGURATION  curWall, 
double  offset) 

{ 

double  J,  deltaTheta,  deltaD; 
double  k=(l. 0/sigma Value () ) ; 
double  a,  b,  c; 

a  *  3 . 0*k; 
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b  =  3 . 0*k*k; 
c  =  k*k*k; 


deltaTheta  =  robot. Theta  -  curWall. Theta; 

deltaD  =  signedDist (robot. Posit,  curWall)  -  offset; 
J  -  -a*robot. Kappa 

-b*norm(  deltaTheta) 

-c*deltaD; 

return (J) ; 


} 


y*********'*********^'*************************'******’*******'********'*’**'********** 
File  name:  queue. c 

******************************************************************************/ 

/* 

This  file  implements  the  (sequential)  instruction  buffer.  All  data 
is  maintained  locally,  so  all  access  is  done  through  function  calls. 

Tue  01-25-94  Frank  Kelbe  -  rewrote  entire  buffer  to  redefine  the  interface 
and  access 

Wed  01-26-94  removed  all  references  to  no_o_paths  and  skip_flag.  "Skipping" 
has  now  been  redefined  to  mean  "flush  the  buffer  as  soon  as  a  new 
instruction  is  ready  to  be  added",  and  now  becomes  a  pseudo-immediate 
command . 

Sun  05-15-94  Cleaned  up  for  inclusion  in  mmlll.  Peek...  function  will  need 
to  be  rewritten  when  the  leaving  point  calculations  are  added  in. 

Fri  07-22-94  FK  -  modified  for  mmlll  and  new  leaving  point  calculations. 

Need  to  be  able  to  always  find  the  next  path  segment  instruction  once 
a  new  instruction  is  processed. 

Mon  08-22-94  FK  -  fixed  routines  in  Processlnstruction  for  tracking  the 
last  path  segment 

Wed  08-24-94  FK  -  fixed  NextSegment  calculations.  Now  NextSegment  is  only 
looked  for  when  the  current  instruction  catches  up. 

V 

#include  "definitions. h" 
tinclude  "system. h" 
tinclude  "memory. h" 

#include  "queue. h" 


/***  Local  types  ***/ 

typedef  struct  inst_element  { 

LINE  path; 

ProcessFunctionPtr  Process;  /*  the  callback  function  pointer  */ 
}  Instruction; 


/***  Local  variables  ***/ 

static 

int 

FlushBuf ferFlag; 

static 

int 

LineCount ; 

static 

LINE 

Buffer [INST  MAX] 

static 

LINE 

*GetPtr, 

*PutPtr, 

*HeadPtr, 

*LastPtr, 

♦NextSegment; 

/***  Local  Prototypes 

***/ 
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static  void  findNextSegment  (void) ; 

static  void  IncPtr(LINE  **); 


/***  Code  ***/ 

/***★*********★*****★*********************************************  * * *  *****  *  ***  * 
PURPOSE:  initialize  the  instruction  buffer 

PARAMETERS :  none 
RETURNS:  void 

COMMENTS:  Wed  01-19-94  Frank  E.  Kelbe 

This  should  only  be  called  one  time  for  initialization 

******************************************************************************* i 


void 

InitQueue (void) 

{ 

HeadPtr  =  &Buffer[0]; 

LastPtr  =  SBuffer [INST_MAX-1] ; 

FlushBuffer () ; 

} 


/****★*★*****************************★***************************************** 
PURPOSE:  Unconditionally  flush  the  instruction  buffer 

PARAMETERS :  none 
RETURNS :  void 

COMMENTS:  Wed  01-19-94  Frank  E.  Kelbe 

******************************************************************************^ 


void 

FlushBuffer (void) 

{ 

PutPtr  =  GetPtr  =  HeadPtr;  /*  reset  to  the  start  of  the  buffer  */ 
LineCount  =0; 

FlushBufferFlag  =  0; 

Next Segment  =  NULL; 

} 


/****************************************************************************** 
PURPOSE:  Used  to  check  the  status  of  the  buffer 

PARAMETERS :  none 

RETURNS:  1  if  the  buffer  is  empty,  0  otherwise 

COMMENTS:  Wed  01-19-94  Frank  E.  Kelbe 

******************************** **********************************************^ 


int 

IsBuf ferEmpty (void) 

{ 

return  (PutPtr  —  GetPtr) ; 

} 

/******************************************************************************* 
PURPOSE:  Effectively  causes  the  buffer  to  be  flushed  once  a  new 

instruction  arrives 
PARAMETERS :  none 
RETURNS:  void 

COMMENTS:  Wed  01-26-94  Frank  Kelbe  replaces  skip() 

******************************************************************************* ^ 
/*  allows  the  queue  to  remember  to  flush  the  buffer  as  soon  as  the  next 
instruction  is  received  and  ready  to  be  added  */ 
void 

SkipAllLines (void) 

{ 

FlushBufferFlag  =  1; 

} 
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/*** ********************************************************************.******** 

PURPOSE:  Flushes  the  buffer,  then  adds  the  next  instruction 

PARAMETERS:  none 

RETURNS :  void 

COMMENTS:  Wed  01-26-94  Frank  Kelbe 

******** ******************* ********* *********** ********************** ********** i 

void 

RestartLine (LINE  path,  ProcessFunctionPtr  Process) 

{ 

FlushBufferFlag  -  1; 

AddLine (path) ; 

} 


/★★★★-a-****:**************************************************** ***************** 

PURPOSE:  see  the  next  segment  to  be  executed. 

PARAMETERS:  a  pointer  to  the  PATHJ2LEMENT  to  be  filled  in 
RETURNS:  a  1  if  there  is  a  path,  otherwise  a  0 

COMMENTS:  Fri  07-22-94  Frank  Kelbe 

revised  Sun  08-21-94 

****************************************************** ************************ j 

int 

PeekNextSegment (LINE  *path) 

{ 

if  (NextSegment)  { 

*path  =  *Next Segment; 
return  1; 

} 

else 

return  0; 


/****************************************************************************** 

PURPOSE:  set  instruction  onto  the  buffer 

PARAMETERS:  path  of  type  PATH  ELEMENT 

RETURNS :  void 

COMMENTS :  Wed  01-19-94  Frank  Kelbe 

************************************* *****************************************  j 

void 

AddLine (LINE  path)  ' 

{ 

/*  while  (LineCount  >=  INST_MAX)*/  /*  wait  for  room  in  the  buffer  */ 

/*  ;  NULL  statement  */ 

if  (FlushBufferFlag)  /*  flush  the  buffer  now  that  we  have  a  new  */ 
FlushBuffer () ;  /*  instruction  to  add  */ 

/*  Disablelnterrupts () ;*/ 

*PutPtr  ^  path;  /*  save  the  path  and  callback  function  */ 

IncPtr ( SPutPtr ) ; 

/ *  LineCount++ ; * / 

/*  Enable  Interrupts  ()•;*/ 

> 


/*****************************************************************+*******+^ 

PURPOSE:  GetLine 

PARAMETERS :  none 

RETURNS :  void 

COMMENTS:  May-97 


115 


**************************************************************  ±  J 

LINE 

GetLine (void) 

{ 

LINE  new; 

new  =  *GetPtr; 

IncPtr ( &GetPtr) ; 

return  new; 

I./**************************************************************************** 

PURPOSE:  ReadLine 
PARAMETERS:  none 
RETURNS :  void 

COMMENTS:  May-97 

****★*★***★*★★****★**★★**★★** ******************************** ★*/ 

LINE 

ReadNextLine (void) 

{ 

LINE  new; 
new  =  *GetPtr; 
return  new; 

} 


/★★★★♦★★★★★★★★★★st*****************************************.***..*.*.**.**.*****..*.**.**. 

PURPOSE:  processes  the  next  sequential  instruction  from 

the  instruction  buffer. 

PARAMETERS:  none 

RETURNS :  void 

COMMENTS:  Thu  01-20-94  Frank  Kelbe 

**★★*+*★**★★*★★*★**★*★*★****★*★★****+***■***•*>*■***■*•**■■*********.+**  i 

/* 

All  of  the  callback  functions  return  an  int,  which  mimics  the 
use  of  flgjmove  in  the  old  code.  If  flgjnove  was  set  ON,  the 
functions  return  1,  otherwise  they  return  0.  A  flag  move  of 
OFF  (return  of  0)  indicates  to  this  function  to  keep  on 
processing  instructions.  If  an  ON  is  returned,  processing 
stops 

*/ 

void 

ProcessLine (void) 

{ 

int  Stopprocessing  =  FALSE; 

LINE  *last; 

while  ( ! IsBufferEmpty ()  &&  ! Stopprocessing) 

{ 

last  =  GetPtr; 

/*  perform  the  callback  passing  a  copy  of  the  data 
to  the  processing  function 

Stopprocessing  =  (* (GetPtr->Process) ) (GetPtr->path) ; */ 

IncPtr (&GetPtr)  ; 

LineCount — ; 

> 

if  (last  ==  NextSegment  | |  ! Next Segment) 

/*  Set  the  file  variable  to  the  next  segment  in  the  queue  */ 
f indNextSegment ( ) ; 
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/**************************** 

Private  Functions 

****************************  j 

static  void 
findNextSegment (void) 

{ 

LINE  *ptr; 

NextSegment  =  NULL;  /*  assume  we  can’t  find  another  path  */ 

/*  No  need  to  look  for  the  next  segment  if  there  are  no  instructions  */ 
if  ( {LineCount  <=  0)  ||  IsBufferEmpty {) ) 
return; 

ptr  -  GetPtr;  /*  start  looking  at  the  next  instruction  */ 

/*  keep  looking  until  there  are  no  more  instructions.  The  checking 
is  done  for  all  elements  in  the  queue,  so  the  last  one  found  is 
the  "next  segment". 

V 

while  (ptr  !=  PutPtr)  { 

/*  ! ! !  This  check  may  have  to  modified  depending  on  how  a 
"next  segment"  is  defined  */ 

if  (  ptr  &&  { 

(ptr->Type  ==  TRACKMODE) 

)) 

{ 

NextSegment  -  ptr; 
break; 

> 

IncPtr (&ptr)  ; 

} 

} 


static  void 
IncPtr (LINE  **ptr) 

{ 

++*ptr; 

if  (*ptr  >  LastPtr) 
*ptr  =  HeadPtr; 

} 


/************************************* ***************************************** 
File  name:  seqcmd.c 

Descriptions:  collection  of  all  of  the  sequential  commands  that  are 
available  to  Yamabico 
Revision  history: 

*********************************** ******************************************* i 


tinclude  "definitions. h” 
#include  "queue. h" 
#include  "seqcmd.h" 
#include  "motion. h" 
#include  "time.h" 
#include  "stdiosys.h" 
#include  <math.h> 


tinclude 

"geometry.h" 

/* 

Added : 

D 

Wells 

4-13-99 

*/ 

#include 

"sonar. h"  /* 

Added:  D  Wells  4-; 

22-99  */ 

# include 

"sonarcard.h" 

/* 

Added : 

D 

Wells 

4-19-99 

*/ 

#include 

"sonarmath.h" 

/* 

Added: 

D 

Wells 

4-19-99 

*/ 

/*  local  variables  */ 

static  MODE  EndStatuslnUser;  /*  used  to  record  the  end  status  of  an  instruction 
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when  it  is  added  to  buffer  in  user  program  */ 


void 

InitSeqcmd (void) 

{ 

EndStatusInUser  *=  STOPMODE; 

} 


void 

track (CONFIGURATION  lineConfig) 

{ 

double  kk; 
double  k,  kk2; 

LINE  pathElement; 

double  sigma  =  sigmaValue  0 ; 

pathElement. Type  =  TRACKMODE; 
pathElement.config  =  lineConfig; 
kk  =  lineConfig. Kappa  ; 

kk2  -  kk  *  kk; 
k  =  1.0/sigma  ; 
pathElement. a  =  3.0*k  ; 
pathElement .b  -  3.0*k*k  -  kk2  ; 
pathElement .c  =  k*k*k  -  3.0*k*kk2  ; 
if  (kk  —  0.0) 

{ 

pathElement .Radius  =  0.0; 
pathElement .Center .X  =  0.0; 
pathElement .Center. Y  =  0.0; 

} 

else 

{ 

pathElement .Radius  =  1.0/kk; 
pathElement .Center .X  -  lineConfig. Posit. X  - 
pathElement . Radius  *  sin (lineConfig. Theta) ; 

pathElement. Cent er.Y  =  lineConfig. Posit. Y  + 
pathElement . Radius  *  cos (lineConfig. Theta) ; 

} 

AddLine (pathElement) ; 
return; 


void 

tracks (CONFIGURATION  lineConfig) 

{ 

double  kk; 
double  k,  kk2; 

LINE  pathElement; 

double  sigma  =  sigmaValue ( ) ; 

pathElement. Type  =  TRACKSMODE; 
pathElement .config  -  lineConfig; 
kk  =  lineConfig. Kappa  ; 

kk2  -  kk  *  kk; 
k  =  1.0/sigma  ; 
pathElement .a  =  3.0*k  ; 
pathElement .b  -  3.0*k*k  -  kk2  ; 
pathElement .c  «  k*k*k  -  3.0*k*kk2  ; 
if  (kk  ==  0.0) 

{ 

pathElement .Radius  =  0.0; 
pathElement .Center .X  =  0.0; 
pathElement. Cent er.Y  =  0.0; 

} 

else 
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{ 

pathElement . Radius  =  1.0/kk; 
pathElement. Center. X  *  lineConfig. Posit. X  - 
pathElement. Radius  *  sin (lineConfig. Theta ) ; 
pathElement. Center. Y  -  lineConfig. Posit .Y  + 
pathElement. Radius  *  cos (lineConfig. Theta) ; 

} 

AddLine (pathElement) ; 
return; 

} 

/************ 

void 

trackWall (int  sonarNum) 

{ 

double  k; 

LINE  pathElement; 

double  sigma  *  sigmaValue () ; 

double  offset; 

/*  sonarNum  determines  side  of  wall  following  */ 

/*  Constants  defined  in  sonar. h  */ 

/*  S090  -  wall  on  right  side,  S270  -  wall  on  left  side  * 
switch  (sonarNum)  { 
case  S090 : 

pathElement . Type  =  TRACKRWALLMODE ; 

Offset  *  DEFAULT_WALL JDFFSET ; 
break; 
case  S270: 

pathElement. Type  =  TRACKL WALLMODE ; 
offset  -  -  DEFAULT_WALL__OFFSET; 
break; 
default: 

print f ("\nERROR:  illegal  value  for  sonarNum  in  trackWall ()") ; 

exit (1) ; 

break; 

} 

pathElement. config  »  defineConfig (0.0,  offset,  0.0,  0.0); 

k  -  1.0/sigma  ; 
pathElement . a  =  3.0*k  ; 
pathElement .b  =  3.0*k*k; 
pathElement. c  =  k*k*k; 
pathElement .Radius  =  offset; 
pathElement. Center .X  -  0.0; 
pathElement. Center. Y  =  0.0; 

AddLine (pathElement) ; 

EnableSonar (sonarNum) ; 

EnableLinearFittingDecay (sonarNum) ; 

return; 

} 

*********** i 


/******** ****************************** ******** ******************************** 
FUNCTION:  Rotate  (sequential) 

PURPOSE:  Rotate  the  robot  by  Theta  radians. 

Positive  is  counterclockwise  and  negative  is  clockwise. 

*************************** **** *********** ****** ********************* ********* f 

void 

Rotate (double  Theta) 

{ 

LINE  path; 

if  (EndStatusInUser  !=  STOPMODE) 

/*  do  nothing  if  it  follows  a  motion  without  stop*/ 
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return 


path. Type  =  ROTATEMODE? 

/*  here  is  where  the  union  would  be  cleaner  */ 
path. config. Theta  =  Theta? 

EndStatusInUser  *  STOPMODE; 

AddLine (path) ; 


#ifdef  CODE_ISJX)NE 

/a***************************************************************************** 

FUNCTION:  set_error  sequential 

PARAMETERS :  code 

PURPOSE:  set  an  error  code  into  the  instruction  buffer. 

RETURNS :  void 

CALLED  BY:  rotate,  solve,  switch_dir,  set_rob 
CALLS :  set_inst ( ) ; 

COMMENTS:  7  Jan  93  -  Dave  MacPherson 

Tue  01-25-94  Frank  Kelbe  -  modified  to  use 

new  buffer 
TASK:  Level  0 

*******★***★★★★★*★★★*★★*★*★***★★*★★★**★★*★*******★***★★★★★*★★**★★***★★*****★★*/ 

void 

set_error ( ERROR_CODE  code ) 

{ 

PATH_ELEMENT  path? 

path. mode  -  ERRORMODE; 
path. type  =  code; 

AddLine (path) ; 

} 


int 

Error Process ( PATH_ELEMENT  path) 

{ 

DispError (path . type ) ? 
halt ( ) ? 

change_status (SERROR) ? 
return  1; 

} 


/*****★*****★*********************★***★**★★★*★**★*★**★***********★***** 
NAME  :  switch_dir 

ARGUMENTS  :  none 

FUNCTION  :  to  reverse  the  heading  direction  of  the  robot 
COMMENTS:  Tue  01-25-94  Frank  Kelbe  -  modified  to  use  new  buffer 

★  *******★****★*★***★**★★***★★★★★*★*★★**★*•*********-*■**■****★★★***  *★★***★*/ 
void 

switch_dir (void) 

{ 

PATH_ELEMENT  path? 

if  (seq^status  !=  SSTOP)  { 

set__error  (ECODE2)  ; 
return? 

} 

path. type  -  SWITCH; 

path.pathType .mode  =  STOPMODE? 

AddLine (path) ? 

nom_p->t  *  norm(nom_jp->t  +  PI); 
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/***********************************************************★*********** 
NAME  :  set^rob 

ARGUMENTS  :  Configuration  to  set  robot’s  location  to. 

FUNCTION  :  to  add  set  robot  sequence  to  queue 

COMMENTS:  Tue  01-25-94  Frank  Kelbe  -  modified  to  use  new  buffer 

***********************************************************************/ 
void 

set^rob (CONFIGURATION  *pst) 

{ 

PATH_ELEMENT  path; 

if (seq_status  !-  SSTOP)  { 
set_error (ECODE2 ) ; 
return; 

} 

path. pc  «  *pst; 
vehicle. x  =  pst->x; 
vehicle. y  =  pst->y; 
vehicle. t  =  pst->t; 
vehicle. k  =  pst->k; 
nomjp->x  -  pst->x; 
nom_p->y  *  pst->y; 
nom__p->t  =  pst->t; 
nom_p->k  -  pst->k; 
path. type  *  SET_ROB; 
path.pathType .mode  =  STOPMODE; 

AddLine (path) ; 

last_robot_path_element.pc  =  *pst; 
last_robot_path_element .  type  =  SET_ROB; 


#endif 


/*************************************************************** 
FILENAME:  sonar. h 

PURPOSE  :  include  file  for  sonar. 

AUTHOR  : 

DATE  : 

COMMENTS:  cleaned  and  documented  by  Mahmoud  Wahdan  and 

Data  structure  LINE_SEG  is  removed  and  added 
to  SEGMENT  and  SEGMENT  changed  to  SEGMENT_RES  & 
mOO  is  added  to  the  new  data  structure.  CUR_DATA 
is  changed  to  SEGMENT_WORK . 

*************************************************************** i 

#ifndef  _ SONAR_H 

#define  _ SONAR_H 

#include  "constants .h" 
tinclude  "definitions.h" 


#define  NUM_SONARS  16 

/*  Sonar  locations  */ 


/* - 

# define 

sooo 

0 

# define 

S030 

3 

#define 

S060 

10 

#define 

S090 

7 
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tdefine  S120  6 

#define  S150  9 

#define  S180  2 

#define  S210  1 

#define  S240  8 

#define  S270  5 

#define  S300  4 

# define  S330  11 


/*  Types  of  sonar  logging  */ 
/* - */ 


#define 

SONAR  NONE 

0x00 

#define 

SONAR  RAW 

0x01 

tdefine 

SONAR  GLOBAL 

0x02 

#define 

SONAR  SEGMENT 

0x04 

#define 

SONAR_ALL 

0x07 

#define 

SONAR_CTL 

0xfc0083f 9 

/*  The  data  structure  SONARD  */ 
/* - */ 


typedef  struct  { 


int 

fitting; 

/* 

flag  to  indicate  linear  fitting  request 

*/ 

int 

decayFitting; 

/* 

flag  to  indicate  linear  fitting  w/  decay 

*/ 

int 

updater- 

/* 

flag  to  indicate  presence  of  new  data 

*/ 

POINT 

global; 

/* 

global  position  of  sonar  return 

*/ 

double 

dr- 

/* 

range  data 

*/ 

double 

dO; 

/* 

old  range  data 

*/ 

POINT 

posit; 

/* 

robot's  position  at  time  of  range 

*/ 

double 

t; 

/* 

robot's  orientation  angle  at  time  of  range 

*/ 

POINT 

SonarPosit; 

/* 

position  of  sonar  from  center 

*/ 

double 

SONARD; 

SonarTheta; 

/* 

angle  of  sonar  from  robot  center 

*/ 

/*  defines  a  basic  segment  with  the  start  and  end  points,  */ 
/*  the  sonar  number  associats  and  the  length  of  segment  */ 
/* - */ 

/*  modified  by  Mahmoud  Wahdan  on  03-06-95  */ 

/* - ★/ 

typedef  struct  { 


int 

SonarNumber ; 

/* 

the  sonar  no  associates  with  a  segment 

*/ 

double 

mOOr- 

/* 

#  points  of  least  squares  LF 

*/ 

POINT 

start; 

/* 

segment  with  the  start  and  end  points 

*/ 

POINT 

end; 

/* 

headx,  heady,  tailx,  taily 

*/ 

double 

alpha; 

/* 

angle  and  length  of  normal  from  origin 

*/ 

double 

r; 

/* 

to  the  segment 

*/ 

double 

SEGMENT 

length; 

RES; 

/* 

length  of  the  segment 

*/ 

/*  defines  a  basic  segment  defined  by  linear  fitting,  */ 


/*  with  a  decay  factor  */ 

/* - v 

/*  modified  by  Dan  Wells  on  04-15-99  */ 

/★ - */ 


typedef  struct  { 

int  SonarNuraber;  /*  the  sonar  no  associates  with  a  segment  */ 

int  usable;  /*  flag  to  indicate  at  least  2  data  points  */ 
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double  numPoints;  /*  #  points  of  least  squares  LF  w/  decay  */ 

double  alpha;  /*  angle  and  length  of  normal  from  origin  */ 

double  r;  /*  to  the  segment  */ 

double  decayFactor;  /*  the  decay  factor  (range:  -0  ->  1)  */ 

/*  1  =>  no  decay,  all  points  equal  weight  */ 

/*  ~0  =>  previous  points  have  no  weight  */ 

double  mOO,  /*  moments  of  linear  fitting  algorithm  */ 


mlO, 

mOl, 

mil, 

m20, 

m02; 

}  SEGMENT_RES_DECAY; 

/*  modified  by  Mark  Merrell  on  1-11-99  */ 
/* - */ 

typedef  struct  { 


int 

SonarNumber; 

/* 

the  sonar  no  associates  with  a 

segment 

*/ 

double 

mOO; 

/* 

#  points  of  least  squares  LF 

*/ 

POINT 

start; 

/* 

segment  with  the  start  and 

end 

points 

*/ 

POINT 

end; 

/* 

headx,  heady,  tailx,  taily 

*/ 

double 

alpha; 

/* 

angle  and  length  of  normal 

from  origin 

*/ 

double 

r; 

/* 

to  the  segment 

*/ 

double 

length; 

/* 

length  of  the  segment 

*/ 

ARRAY  RES; 

/*  revised  by  Y.  Kanayama,  07-07-93  */ 

/* - */ 

typedef  struct  { 

double  mOO,  /*  moments  of  least  squares  fitting  algotithm  */ 

mlO, 
mOl, 
m20, 
mil, 
m02; 

SEGMENT_RES  seg;  /*  sonar  no.,  startx,  starty,  endx,  endy,  */ 

/*  alpha,  r,  length  */ 

}  SEGMENT  WORK; 


/*  Global  variables  */ 

/* - */ 

extern  SONARD  sonar_table [ ] ; 

extern  ARRAY_RES  SegmentArray [NUM_SONARS] [100] ; 

/*  Prototype  */ 

/* - */ 

void  InitSonar (void) ; 


/*  Interrupt  handler  */ 

/* - */ 

void  SonarSysControl (void) ; 

tendif 


/******************-k±**1'*i'*i'***-k*i'*i'1r±*****ir*-k1'i(ie****ie*****i'’ki'-ki'*ir***1'ici' 

FILENAME  :  sonar . c 

PURPOSE  :  Provides  the  global  generic  sonar  functions 

CONTAINS  :  InitSonar () 
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SonarSysControl { ) 


AUTHOR  :  Patrick  Byrne,  Yutaka  Kanayama 

DATE  :  20  November  1993 

COMMENTS  :  Fri  07-22-94  Updated  for  Sparc  ntmlll  FEK 

:  updated  by  Khaled  morsy  11-22-94 

:  cleaned  and  documented  on  Mon  3-6-96  by  Mahmoud  Wahdan 
**************************★**********★****★******★****★*★*★★★****★**★★*★/ 


#include 
#include 
# include 
#include 
#include 
#include 
#include 
#include 
tinclude 
#include 


"definitions. hn 
"memsys.h" 
"sonar. h" 
"motion. h" 
"sonarcard.h" 
"sonarmath.h" 
"sonarlog.h" 
"spare. h" 
"time.h" 

"trace. h" 


#ifdef  TIMER 
tinclude  "trace. h" 
#include  "clocktick.h" 


#define  TIMER_FILE 
tdefine  TIMERJ5IZE 
#define  TIMER_FREQUENCY 


"timer.log" 

0x10000 

1 


IOhandle  TimerLog; 
#endif 


/*  Global  variables  */ 
/* - */ 


SONARD  sonar_table[NUM_SONARS] ;  /*  SONARD  struc  &  NUM^SONARS  =  16  */ 

/*  are  defined  in  sonar. h  */ 

ARRAY_RES  SegmentArray [NUM_SONARS]  [100]; 

/*  used  by  ServeSonar  */ 

/* - */ 

static  const  int  group_array [4] [4]  =  /*  array  maps  sonar  no  to  groups  */ 

{ 

{0,  5,  2,  7}, 

{3,  4,  1,  6}, 

{10,  11,  8,  9}, 

(12,  13,  14,  15} 


/★*•★****★*★*★****★*****★**★*****•*•*★*★★*★**★★*******★★**★★★★★★★★★★★★**★★****★★★* 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 

CALLS 


CALLED  BY 
COMMENTS 


InitSonar ( ) 

None 

initializes  sonar  table  and  sets  up  compensation  for  sonar 

configurations . 

void 

InitSonarmath ( )  in  sonarmath.h. 

InitSonarlog ( )  in  sonarlog.h. 

SetSonarParameters ()  in  sonarmath.h. 
memsetO  in  memsys.h. 

None 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


★  *********★**★**★*★**★***★**★********★****★★****★★*****★★■*■★★*★***★■*■****★★★★*★** 


/ 


void 

InitSonar (void) 

{ 

int  i ; 
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/*  initialize  sonar^table  */ 
/* - */ 


for  (i  =  0;  i  <  NUM_SONARS;  i++)  /*  NUM_SONARS  =  16  is  defined  in  sonar. h  */ 

{ 

memset (&sonar_table [i] ,  0,  sizeof (SONARD) ) ; 
sonar_table [i] . fitting  -  0; 
sonar_table [i] .decayFitting  =  0; 
s onar_t able [i] .update  -  0; 
sonar__table  [i]  .d  =  0.0; 

sonar_table [i] .d0  =  0.0; 

sonar_table [i] .global. X  =  0.0; 
sonar_table[i] .global. Y  =  0.0; 
sonarjtable  [ij .t  =  0.0; 

sonar_table [i] .posit .X  =  0.0; 
sonar_table [i] .posit. Y  =  0.0; 

> 

/*  set  up  compensation  for  new  sonar  configurations  12/08/94  */ 

/* - */ 


sonar 

sonar 

sonar 

sonar 

sonar 

sonar' 

sonar 

sonar 

sonar 

sonar 

sonar 

sonar 

sonar 

sonar 

sonar' 

sonar 


table [0] . 
table [1] . 
"table  [2]  . 
‘table  [3] . 
‘table  [4] . 
'table  [5]  . 
"table  [6]  . 
"table  [7]  . 
"table  [8]  . 
'table  [9]  . 
table [10] 
“table  [11] 
"table [12] 
'table  [13] 
table [14] 
'table  [15] 


SonarTheta  = 
SonarTheta  = 
SonarTheta  = 
SonarTheta  = 
SonarTheta  = 
SonarTheta  = 
SonarTheta  * 
SonarTheta  = 
SonarTheta  = 
SonarTheta  = 
. SonarTheta 
. SonarTheta 
. SonarTheta 
.SonarTheta 
. SonarTheta 
. SonarTheta 


0.0; 

5.0*PI/6.0; 

PI; 

-PI/6.0; 

PI/3.0; 

PI/2.0; 

-2 . 0*PI/3 . 0; 
-PI/2.0; 

2 . 0*PI/3 . 0; 
-5 . 0*PI/6.0; 
=  -PI/3.0; 

=  PI/6.0; 

=  0.0; 

=  1.5708; 

=  4.7124; 

=  0.0; 


sonar_table [0] .SonarPosit.X  -  23.6; 
sonar_table [1] .SonarPosit.X  =  -23.0; 
sonar_table [2] .SonarPosit  .X  =  -22.6; 
sonarjtable [3] .SonarPosit.X  =  24.7; 
sonarjtable [4] .SonarPosit.X  =  13.4; 
sonar_table [5] .SonarPosit.X  =  0.0; 
sonar_table [6] .SonarPosit.X  =  -12.6; 
sonar_table [ 7 ] . Sonar Posit .X  =  0.0; 
sonar_table [8] .SonarPosit.X  =  -13.4; 
sonar_table [9] .SonarPosit.X  -  -23.5; 
sonar_table [10] .SonarPosit.X  =  12.1; 
sonar__t able [11]  .SonarPosit.X  =  25.2; 
sonarjtable [12] .SonarPosit.X  =  0.0; 
sonar_table [13] .SonarPosit.X  =  1.5708; 
sonar_table [14] .SonarPosit.X  =  4.7124; 
sonar_table[ 15] .SonarPosit.X  -  0.0; 


sonar_table [0] . SonarPosit . Y  =  -0.5; 
sonarjtable [1] . Sonar Posit. Y  =  13.1; 
sonar_table [2] .SonarPosit. Y  =  -1.0; 
sonar_table [3] .SonarPosit. Y  =  -14.6; 
sonar_t able [4] . SonarPosit. Y  -  21.3; 

sonar_table [5] . SonarPosit. Y  =  20.6; 
sonar_table{6] .SonarPosit. Y  -  -21.3; 
sonar_table [7] .SonarPosit .Y  =  -20.5; 
sonar_table [8] . SonarPosit .Y  =  21.3; 
sona r_t able [9] . Sonar Posit. Y  =  -14.9; 
sonar_table [10] .SonarPosit. Y  =  -21.3; 
sonar_table [11] .SonarPosit. Y  -  14.1; 
sonar_table [12] .SonarPosit. Y  =  0.0; 
sonar_table [13] .SonarPosit. Y  =  21.5; 
sonar_table [14] .SonarPosit .Y  =  21.5; 
sonar_table [15] .Sonar Posit. Y  =  0.0; 
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/*  initialize  the  sonar  components  */ 
/* - */ 


InitSonarmath ( ) ;  /*  it  is  defined  in  sonarmath.h  */ 

InitSonarlog ( ) ;  /*  it  is  defined  in  sonarlog.h  */ 

Set Sonar Parameters (0 .02,  5.0);  /*  it  is  defined  in  sonarmath.h  */ 

#ifdef  TIMER 

TimerLog  =  IOopen (TIMER_FILE,  TIMER  SIZE,  TIMER  FREQUENCY); 

#endif 


} 


/************************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 

data 


SonarSysControl ( ) 

None 

It  is  the  ''central  command* 1  for  the  control  of  all  sonar  related 
functions.  It  is  linked  with  the  ih  sonar  routine  and  loads  sonar 


:  to  the  sonar_table  to  determine  which  activities  the  user  wishes  to 
:  take  place,  and  calls  the  appropriate  functions.  This  procedure  is 
:  invoked  approximately  every  thirty  msec  by  an  interrupt  from  the 

sonar 


RETURNS 

CALLS 


CALLED  BY 
COMMENTS 


:  control  board. 

:  None 

:  CalculateGlobal ( )  in  sonarmath.h. 

:  LinearFitting ()  in  sonarmath.h. 

:  LinearFittingDecay ()  in  sonarmath.h. 

:  LogSonarData {)  in  sonarlog  .h. 

:  changeLEDstate ()  in  system. h. 

:  getRobotConf ig ( )  in  motion. h. 

:  None 

;  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


**★***★★***★**★*★***★*■**★*********************■*■■**  ***★*********★★★•**■**★•**********■*****/ 


void 

SonarSysControl (void) 

{ 


#define  OVERFLOWMASK 
tdefine  GROUP_MASK 
tdefine  SONAR_DATAO 
tdefine  SONAR_DATAl 
tdefine  SONAR  JDATA2 
tdefine  SONAR  DATA3 


0x8000 

0x18 

0xfc0083f 0 
0xfc0083f2 
0xfc0083f 4 
0xfc0083f 6 


/*  overflow  bit  is  bit  15  */ 


static  int 

int 

int 

int 

int 

/*  double 

CONFIGURATION 


cnt  =  0; 
n; 
i ; 

data [ 4 ] ; 
group; 

t;  */ 

current; 


/*  uses  only  in  TIMER  */ 

/*  is  defined  in  definition. h  */ 


tifdef  TIMER 

int  tick  -  getCount ( ) ; 

IOprintf (TimerLog,  "%f  ",  tick  /  250.0); 
tendif 


if  (++cnt  >  10)  /*  blink  the  tl  LED  */ 

{ 

cnt  =  0; 

changeLEDstate (1)  ;  /*  is  defined  in  system. h  */ 

} 

current  =  getRobotConf ig () ;  /*  getRobotConf ig ( )  is  defined  */ 

/*  motion. h  */ 
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group  «  ( (* (BYTE*) SONAR_CTL  &  GROUP J4ASK)  »  3); 

datatO]  =  * (WORD* ) SONAR_DATAO ; 
data  [1]  =  *  (WORD* )  SONAR__DATAl ; 
data [2]  =  *  (WORD* )  SONAR_DATA2 ; 
data  [3]  =  *  (WORD*)  SONAR__DATA3  ; 

for  (i  =  0;  i  <  4;  i++) 

{ 

n  =  gr oup_ar ray [group] [i] ; 

sonar_table [n] .posit .X  =  current. Posit. X; 
sonar__table[n]  .posit. Y  =  current. Posit .Y; 
sonar_table [n] .t  =  current .Theta; 
sonar_table [n] ,d0  =  sonar_table [n] .d; 

if  (data [i]  &  OVERFLOWMASK) 

sonar_table[n] .d  =  INFINITY; 
else 
{ 

data [i]  &=  Oxfff;  /*  only  first  12  bits  are  data,  */ 

/*  so  mask  the  data  */ 

sonarjtable [n] .d  =  data[i]  *  0.1029; 

} 


sonar_table [n] .update  =  1; 


CalculateGlobal (n) ; 

if  (sonar_table [n] . fitting  ==  1) 
LinearFitting(n) ; 


/* 

is 

defined 

in 

sonarmath.c 

*/ 

/* 

is 

defined 

in 

sonarmath.c 

*/ 

if  (sonar_table [n] .decayFitting  --  1) 

LinearFittingDecay (n) ;  /*  is  defined  in  sonarmath.c  */ 

/*  LogSonarData (n) ;  */  /*  this  function  is  moved  to  */ 

/*  CalculateGlobal ( )  in  sonarmath.c  */ 

/*  because  interrupt  affects  it  &  some  */ 

/*  logging  numeric  digits  becomes  character  */ 
/*  done  by  Mahmoud  wahdan  Aug.  16  95  */ 


#ifdef  TIMER 

t  =  (tick  -  getCount () ) /250.0; 
if  (t  <  0.0) 

t  =  t  +  10.0; 

IOprintf (TimerLog,  "%f  \n",  t  ); 
#endif 


} 


/********★************************************************************** 


FILENAME 

:  sonarcard.c 

PURPOSE 

:  contains  functions  for  sonar. 

CONTAINS 

:  InitSonarcard ( ) 

Enable Sonar () 

DisableSonar () 

DisableAllSonar ( ) 
EnableSonarlnterrupts () 
DisableSonarlnterrupts ( ) 

AUTHOR 

:  Patrick  Byrne 

DATE 

:  20  November  1993 
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COMMENTS 


:  Fri  07-22-94  Updated  for  Sparc  mmlll  FEK 
:  cleaned  and  documented  on  Mon  3-6-96  by  Mahmoud  Wahdan 


# include 
#include 
#include 
#include 
# include 
# include 
# include 
# include 


"definitions .h" 
"sonar .h" 
"system. h" 

"time .h" 
"sonarcard.h" 
<io_defs.h> 
<reg.h> 
ctraptypes .h> 


/******  Local  variables  ******/ 
/******  _  ****** ^ 


static  char 
static  char 
static  int 
static  char 


*command_ptr; 

*BIM_ptr; 

enabled_sonars [NUM__SONAR$] ; /*  NUM_SONAR  =  16  is  defined  in  sonar. h  */ 
enabled; 


#define 

SONAR__VECTOR 

#define 

BIM0_CTL0 

#define 

BIM0_VECT0 

#define 

IRQ2 

#define 

SONAR_CTL 

#define 

IRQ2_REG 

0xa2 

0xfc0080cl  /*  BIMO  control  register  */ 

0xfc0080c9  /*  BIMO  vector  register  */ 

0x12 

0xfc0083f 9  /*  sonar  board  command/status  register  */ 

OxfffcOOOb 


int 

SonarSysControlWrapper (TrapType  trap,  struct  reg_save  *reg,  int  vector) 

{ 


SonarSysControl ( ) ; 
return  IO_INTR_EXPECTED; 

} 


/******  Source  Code  ******/ 

/******  -  ****** i 


j ************************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 

register. 

RETURNS 

CALLS 


CALLED  BY 
COMMENTS 


InitSonarcard ( ) 

None 

It  initializes  enabled_sonars  list  and  sonar  board  command/status 
void 

Disablelnterrupts ( )  in  system. h. 

Enablelnterrupts ()  in  system. h. 
mk_handler()  in  spare. h. 

None 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


************************************************************************************* j 


void 

InitSonarcard (void) 

{ 

int  i  ; 

#define  SonarlntVector  0xa2 

command_ptr  -  (char  *)SONAR_CTL; 
BIM_ptr  =  (char  *)BIM0_CTL0; 
enabled  =0; 
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for  (i=0;  i<NUM_SONARS  ;  i++) 
enabled  sonars [i]  =  0; 


/*  Initialization  */ 


Disablelnterrupts ( ) ;  /*  defined  in  system. h  */ 

/*  This  function  is  not  defined  */ 

mk_handler  (Sonar IntVect or,  SonarSysControlWrapper)  ;  /*  defined  in  spare. h  */ 

*  (BYTE*)  BIM0__VECT0  =  SONAR_VECTOR; 

* (BYTE*) BIM0_CTL0  -  IRQ2; 

*/ 

* (BYTE*) SONAR_CTL  =  8; 

* (BYTE*) IRQ2_REG  «  0x7a; 

Enablelnterrupts ( ) ;  /*  defined  in  system. h  */ 

/*************  if*************************************************** **************  ****** 

FUNCTION  :  EnableSonar (SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  enables  the  sonar  group  that  contains  sonar  SonarNumber,  which 

causes 

:  all  the  sonars  in  that  group  to  echo-range  and  write  data  to  the  data 
:  registers  on  the  sonar  control  board.  Makes  the  SonarNumber ’ th 

position 

:  of  the  enabled_sonars  array  to  track  which  sonars  are  enabled. 

RETURNS  :  void 

CALLS  :  DisableSonarlnterrupts ( )  in  sonarcard.h. 

CALLED  BY  :  None 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

************************************************************************************* y 

void 

EnableSonar (int  SonarNumber) 

{ 

DisableSonarlnterrupts ( )  ; 

enabled_sonars [SonarNumber] 

switch  (SonarNumber)  { 
case  0: 
case  2: 
case  5: 
case  7: 

enabled  1=  0x01; 
break; 
case  1: 
case  3: 
case  4: 
case  6: 

enabled  |=  0x02; 
break; 
case  8: 
case  9: 
case  10: 
case  11: 

enabled  |=  0x04; 
break; 
case  12: 
case  13: 
case  14: 
case  15: 

enabled  |=  0x08; 
break; 

}  /*  switch  */ 

EnableSonarlnterrupts  ( ) ;  /*  defined  in  sonarcard.h  */ 

*command_ptr  =  enabled; 

> 

/***************************************★********************************************* 

FUNCTION  :  DisableSonar (SonarNumber) 


/*  enable  interrupts  and  select  IRQ2 
/*  zeroes  the  command  register  */ 
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PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  removes  the  sonar  SonarNumber  from  the  enabled  sonars  list.  If 


sonar 


RETURNS 

CALLS 

CALLED  BY 
COMMENTS 
★  *★★*****★★*•*■*★ 


:  SonarNumber  is  the  only  enabled  sonar  from  it's  group,  then  the  group 
:  is  disabled  as  well  and  will  stop  echo  ranging.  This  has  benefit  of 
:  shortening  the  ping  interval  for  groups  that  remain  enabled. 

:  void 

:  DisableSonarlnterrupts ()  in  sonarcard.h 
:  EnableSonarlnterrupts ( )  in  sonarcard.h. 

:  DisableAll Sonar ()  in  sonarcard.h. 

:  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 
*********★*****★*★★**********★***★*★**********************★********★**/ 


void 

DisableSonar ( int  SonarNumber ) 

{ 

int  c; 


DisableSonarlnterrupts () ;  /*  defined  in  sonarcard.h  */ 

enabled_sonars [SonarNumber]  =  0; 


switch  (SonarNumber) 

{ 

case  0: 
case  2: 
case  5: 
case  7: 

c  =  enabled__sonars  [0]  +  enabled_sonars  [2]  + 
enabled_sonars [5]  +  enabled_sonars [7] ; 
if  (c  ==  0) 

enabled  &=  Oxfe; 
break; 
case  Is 
case  3: 
case  4: 
case  6: 

c  =  enabled_sonars [1]  +  enabled_sonars [3]  + 
enabled__sonars  [4]  +  enabled_sonars [6] ; 
if  (c  ==  0) 

enabled  &=  Oxfd; 
break; 
case  8: 
case  9: 
case  10: 
case  11: 

c  =  enabled_sonars [8]  +  enabled_sonars [9]  + 
enabled_sonars [10]  +  enabled_sonars [11] ; 
if  (c  0) 

enabled  &=  Oxfb; 
break; 
case  12: 
case  13: 
case  14: 
case  15: 

c  =  enable d_sonars [12]  +  enabled_sonars [13]  + 
enabled_sonars [14]  +  enabled_sonars [15] ; 
if  (c  -=  0) 

enabled  &=  0xf7; 
break; 

}  /*  switch  */ 


*commandjptr  =  enabled; 

EnableSonarlnterrupts () ;  /*  defined  in  sonarcard.h  */ 


/*************★★******★*****************************************★*******★*******★***** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 

CALLS 


DisableAllSonar ( ) 

None 

removes  the  all  sonars  from  the  enabled_sonars  list, 
void 

DisableSonar ()  in  sonarcard.h. 
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CALLED  BY  :  None 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

*************************************************************************************y 


void 

DisableAllSonar (void) 
{ 

int  i; 


} 


for  (i=0;  i  <  12;  i++) 
DisableSonar (i) ; 


/*  defined  in  sonarcard.h  */ 


/★★a********************************************************************************** 

FUNCTION  :  EnableSonarlnterrupts ( ) 

PARAMETERS  :  None 

PURPOSE  :  places  sonar  control  board  in  interrupt  driven  mode. 

RETURNS  :  void 

CALLS  :  None 

CALLED  BY  :  None 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

**************************** *********************************************************^ 


void 

EnableSonarlnterrupts (void) 

{ 

*BIM_ptr  |=  0x10;  /*  cordeiro  code  has  *BIM_ptr  =  0x12;  */ 


/************************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


DisableSonarlnterrupts ( ) 

None 

stops  interrupt  generation  by  the  sonar  control  board.  A  flag  is  set 

in  the  status  register  when  data  is  ready,  and  it  is  the  user's 

responsibility  to  poll  the  sonar  system  for  the  flag. 

void 

None 

None 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


******************************** ****************************************************.fcy 


void 

DisableSonarlnterrupts (void) 

{ 

*BIM_ptr  &-  Oxef ;  /*  cordeiro  code  has  *BIM_ptr  ~  0;  */ 


/*********************************************************************** 
FILENAME  :  sonarmath.c 

PURPOSE  :  Provides  the  main  sonar  functions 

CONTAINS  :  InitSonarmath ( ) 

Sonar ( ) 

SonarNew ( ) 

Global ( ) 

CalculateGlobal { ) 

SetSonarParameters ( ) 

EnableLinearFitting ( ) 

DisableLinearFitting ( ) 

LinearFitting ( ) 

AddToSegment ( )  *  this  function  is  local  in  sonarmath.c  * 

GenerateSegment ( ) 

EndSegment ( ) 

BuildListO  *  this  function  is  local  in  sonarmath.c  * 

ResetMoments ( )  *  this  function  is  local  in  sonarmath.c  * 

GetLastSegment ( ) 

GetCurrentSegment ( ) 
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GetSegmentConf ig { ) 

WaitSegment ( ) 

CorrectOdometryError ( ) 

**  Linear  Fitting  with  Decay  ** 

**  Added  by  Dan  Wells  on  04-15-99  ** 
ResetMomentsDecay ( ) 
EnableLinearFittingDecay ( ) 
DisableLinearFittingDecay ( ) 
LinearFittingDecay ( ) 
AddToSegmentDecay ( ) 

GetSegmentDecay ( ) 

SetDecayFactor ( ) 


AUTHOR  :  Patrick  Byrne 

DATE  :  20  November  1993 

COMMENTS  :  Fri  07-22-94  Updated  for  Sparc  mmlll  FEK 

:  cleaned  and  documented  on  Mon  3-6-96  by  Mahmoud  Wahdan 
:  and  data  structure  LINE-SEG  is  removed  and  added  to  SEGMENT_RES. 
:  and  CUR_DATA  is  changed  to  SEGMENT_WORK. 

:  Refer  to  this  data  structure  in  sonar. h 

****** **************** ******* *********************** ****** ****** ********** i 


#include  "definitions .h" 
iinclude  "stdiosys.h" 

#include  "sonar. h” 

#include  "math.h” 

#include  "motion. h" 

#include  "memsys.h" 

#include  "sonarlog.h" 
tinclude  "sonarmath.h" 

#include  "trace. h” 
tinclude  "geometry. h" 

/♦ADDED*/ 

#include  "spare. h" 

#define  SIZE  30 

/*  #define  DEBUG  */ 

/* 

*  the  following  constants  are  used  in  logging  functions  for  debugging 

*  purposes 
*/ 

#ifdef  DEBUG 
#define  DEBUG_FILE 
#define  DEBUG_SIZE 
#de fine  DEBUG_FREQUENCY 

IOhandle  debuglO; 

#endif 


"debug.log" 

0x10000 

1 


/*  #define  LOCALIZATION  */ 

/* 

*  the  following  constants  are  used  in  logging  functions  for  localization  purpose  */ 
#ifdef  LOCALIZATION 

fdefine  LOCALIZATION_FILE  "localization . log" 

#define  LOCALIZATIONDEBUG_SIZE  0x10000 

#define  LOCAL I ZATION_FREQUENCY  1 

IOhandle  localizationlO; 

#endif 
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/*  Local  variables  */ 
/* - */ 


static  double  Cl,  C2; 

static  SEGMENTJRES  segstruct  ; 


/*static  int 

static  int 

static  SEGMENT  RES 


SegListHead [NUMJ50NARS] ;*/ 
SegListTail [NUM_SONARS] ; 
seg_list [NUM_SONARS] [SIZE] ; 


/*  temporary  storage  for 
EndSegment  func.*/ 

/*  points  to  oldest  segment 
array  element  */ 

/*  points  to  newest  segment 
array  element  */ 

/*  segments  for  working 
memory  */ 


static  SEGMENTJffORK  segment_data  [NUM_SONARS]  ; 


/*  interim  data  for  all 
sixteen  sonars  CURJ5ATA 
is  changed  to  SEGMENT  JiFORK  */ 


static  SEGMENT_RES_DECAY  seg_data_decay [NUM_SONARS] ;  /*  data  for  sonars  using 

linear  fitting  with 
decay  factor  */ 

static  SEGMENT_RES  *seg_end_conf ig;  /*  temporary  storage  for 

segment  being  built  is 
completed  */ 

/*  Local  Prototypes  */ 

/* - */ 

void  AddToSegment {int,  POINT); 

void  ResetMoments  (int)  ; 

void  BuildList  (SEGMENT_RES* ,  int)  ; 

void  AddToSegmentDecay (int,  POINT); 


/*  Source  Code  */ 
/* - */ 


/****************************************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 

CALLS 

CALLED  BY 

COMMENTS 


:  GetSegment (int  SonarNumber,  int  SegmentNumber) 
:  int  SonarNumber,  int  SegmentNumber 
:  Gets  a  particular  segment 
:  SEGMENTJRES 
:  None 
:  user.c 

:  Created  by  Mark  Merrell  20  May  1998 


************* ********************************* *********************** ********** 


/ 


SEGMENTJRES  * 

GetSegment (int  SonarNumber,  int  SegmentNumber) 

{ 

SEGMENTJRES  Segment; 


Segment  -  segJList  [SonarNumber]  [SegmentNumber] ; 


return  &  Segment; 


} 


/***★*******■**************************.*******.*********************************** 


FUNCTION 

PARAMETERS 

PURPOSE 


GetNiamPoints  (int  SonarNumber) 
int  SonarNumber 

Gets  a  number  of  points  in  the  current  segment 
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RETURNS  :  int 

CALLS  :  None 

CALLED  BY  :  user.c 

COMMENTS  :  Created  by  Mark  Merrell  14  July  1998 

★★★a******************************************************************-********* i 

int 

GetNumPoints {int  Sonar Number) 

{ 

return  segment_data [SonarNumber] .mOO; 


/***★*************★★***★****★**★*★★*****************★***************★*********** 
FUNCTION  :  GetNumSegments {int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  Returns  the  number  of  Segments 

RETURNS  :  int 

CALLS  :  None 

CALLED  BY  :  user.c 

COMMENTS  :  Created  by  Mark  Merrell  20  May  1998 

int 

GetNumSegments (int  SonarNumber) 

{ 

return  SegListTail [SonarNumber] ; 

} 

FUNCTION  :  InitSonarmath ( ) 

PARAMETERS  :  None 

PURPOSE  :  It  resets  the  accumulative  values  mOO,  mlO,  mOl,  m20,  mil, 

m02  to  zero. 

RETURNS  :  void 

CALLS  :  memsetO  in  memsys.h. 

:  ResetMoments ( )  in  sonarmath.c  *Local  Prototype's  in  sonarmath.c* 
:  ResetMomentsDecay ( )  in  sonarmath.c  *Local  Prototype's  in 

sonarmath.c* 

CALLED  BY  :  InitSonarO  in  sonar. h. 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

*******************************************************************************  j 

void 

InitSonarmath (void) 

{ 

int  i; 

for  (i  =  0;  i  <  NUM_SONARS;  i++) 

{ 

ResetMoments (i) ; 

ResetMomentsDecay (i) ; 

SegListTail [i]  =  -1; 


segment_data[i] .seg. start. X  =  0.0 
segment_data [i) . seg. start .Y  =  0.0 
segment__data [ij  . seg.end.X  =  0.0 
segment_data[i] .seg.end. Y  -  0.0 
segment_data [i] . seg. alpha  =0.0 
segment_data[i] .seg.r  =  0.0 
segment_data [i] . seg. length  =  0.0 
segment_data[i] .seg.mOO  =  0.0 


segment_data[i] .seg. SonarNumber  =  i; 

} 


/*  NUM_S ONARS  16  is  defined  in  sonar. h  */ 

/*  defined  in  sonarmath.c  */ 

/*  defined  in  sonarmath.c  */ 
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} 

/********************  source  Code  For  Sonar  ***************************/ 
/*  =================  */ 


/**************************★*************************************************** 
FUNCTION  :  Sonar (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  returns  the  distance  (in  centimeters)  sensed  by  the 

:  SonarNumber  ultrasonic  sensor. 

:  If  no  echo  is  received, 

:  then  INFINITY  (1.0$e$6)  is  returned. 

:  If  the  distance  is  less  than  10  cm, 

:  then  a  0  is  returned. 

RETURNS  :  the  distance  (in  centimeters)  sensed  by  the  SonarNumber 

:  ultrasonic  sensor. 

CALLS  :  None 

CALLED  BY  :  None 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

***********************************★*******************************★*****★*★**/ 

double 

Sonar (int  SonarNumber) 

{ 

sonar_table [SonarNumber] .update  =  0; 

return  sonar_table [SonarNumber] .d;  /*  sonar_table  is  global  variable  which 

is  defined  in  sonar. c 

the  data  structure  of  sonar_table  is 

SONARD  which  is  defined  in  sonar. h  */ 

} 

/**■**  **************************************  **************************  ************ 
FUNCTION  :  SonarNew(int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  waits  in  a  loop  until  new  data  ia  available  for  sonar  SonarNumber. 

RETURNS  :  the  range  value  for  sonar  number. 

CALLS  :  None 

CALLED  BY  :  None 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

******************************************************************************  ^ 

double 

SonarNew(int  SonarNumber) 

{ 

while  (sonar_table [SonarNumber] .update  ==  0) 

;  /*  NULL  statement  */ 

sonar_table [SonarNumber] .update  =  0; 
return  sonar_table [SonarNumber] .d; 

} 

/****************************************************************************** 
FUNCTION  :  Global (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  returns  a  structure  of  type  POINT  containing  the  global  x 

:  and  y  coordinates  of  the  position  of  the  last  sonar  return. 

RETURNS  :  the  global  x  and  y  coordinates  of  the  position  of  the  last 

:  sonar  return. 

CALLS  :  None 

CALLED  BY  :  None 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

******************************************************************************/ 

POINT 

Global (int  SonarNumber) 

{ 

return  sonar_table [SonarNumber] .global; 

} 
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/************★★**★***★★***★**************************************************** 

FUNCTION  :  CalculateGlobal (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  calculates  the  global  x  and  y  coordinates  for  the  range  value 

:  and  robot  configuration  in  the  sonar  table.  The  results  are 
:  stored  in  the  sonar  table. 

RETURNS  :  None 

CALLS  :  getRobotConfigO  in  motion.h. 

:  LogSonarData ( )  in  sonarlog.h 

:  Disablelnterrupts ()  in  spare. c 

:  EnaJolelnterrupts  ()  in  spare. c 

CALLED  BY  :  SonarSysControl ( )  in  sonar. h. 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

:  Disablelnterrupts (),  Enablelnterrupts ( )  &  LogSonarData ( )  added  by 
:  Mahmoud  wahdan  on  Aug  16,95  to  solve  the  problems  of  logging  data 
:  (some  numeric  digits  becomes  characters  and  some  numbers  becomes 
:  very  big  or  very  small  (wrong  calculation) . 
★************************************************************************** ***/ 

void 

CalculateGlobal (int  SonarNumber) 

{ 

double  lx,  ly.  It,  range; 
double  sonarX,  sonarY,  sonarTheta; 
double  robotX,  robotY,  robotTheta; 


Disablelnterrupts ( ) ; 

range  =  sonar__table  [SonarNumber]  .d; 

if  (range  >=  INFINITYO) 

{ 

sonar_table [SonarNumber] .global. X  =  INFINITY;  /*  sonar  table  is  global  variable 

*/ 

sonar_jtable [SonarNumber] .global .Y  *  INFINITY;  /*  which  is  defined  in  sonar. c  */ 

}  /*  the  structure  of  sonar_table  */ 

else  /*  is  SONARD  which  is  defined  in 

*/ 

{  /*  sonar. h  */ 

sonarX  =  sonar_table [SonarNumber]  .SonarPosit .X; 
sonarY  -  sonar_table [SonarNumber] .SonarPosit .Y; 
sonarTheta  =  sonar_table [SonarNumber) .SonarTheta; 

robotX  =  sonar_table [SonarNumber] .posit .X; 
robotY  =  sonar_table [SonarNumber] .posit .Y; 
robotTheta  =  sonar_table [SonarNumber] .t; 

/*  vehicle  compose  sonar  */ 

/* - */ 

/*  global  sonar  config  =  global  robot  config  o  local  sonar  config  */ 


/* - */ 

/*  |  lx  |  | robotX  |  | cos (robotTheta)  -sin (robotTheta)  0|  | sonarX  | 

J  ly  |=| robotY  | + j sin (robotTheta)  cos (robotTheta)  0|*1 sonarY  | 

!  It  1  | robotTheta |  |  0  0  1|  | sonarTheta! 

Ill  II  II  |  */ 

lx  =  robotX  +  (cos (robotTheta)  *  sonarX)  -  (sonarY  *  sin (robotTheta) ) ; 
ly  =  robotY  +  (sin (robotTheta)  *  sonarX)  +  (sonarY  *  cos (robotTheta) ) ; 
It  =  sonarTheta  +  robotTheta; 


/*  vehicle  compose  sonar  range  */ 
/* - */ 


/*  describes  the  sensor* s  configuration 
in  the  global  coordinate  system  */ 

/*  describes  the  sensor’s  configuration 
in  the  local  coordinate  system  */ 

/*  describes  the  robot  current  configuration 
in  the  global  coordinate  system  */ 
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sonar_table [SonarNumber] .global. X  -  lx  +  (cos(lt)  *  range); 
sonar_table [SonarNumber] .global. Y  =  ly  +  (sin(lt)  *  range) ; 


LogSonarData  (SonarNumber) ; 
Enablelnterrupts ( ) ; 


/*********************  Source  Code  For  Linear  Fitting  *************************/ 

/  *  ==========r==*================  *  / 

/********************************** ********************************************* 
FUNCTION  :  SetSonarParameters (double  cl,  double  c2) 

PARAMETERS  :  double  cl 

double  c2 

PURPOSE  :  It  allows  the  user  to  adjust  constants  which  control  the 

linear  fitting  algorithm. 

cl  is  a  multiplier  to  allow  more  lenancy  for  greater  sonar 
ranges.  c2  is  an  absolute  value,  cl  and  c2  are  used  to 
determine  if  an  individual  data  point  is  usable  for  the 
algorithm. 

Default  values  are  set  in  main.c  to  0.02,  5.0  respectively, 
void 
None 

InitSonarO  in  sonar. h. 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

*******************************************************************  i 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 
*********** 


void 

SetSonarParameters (double  cl,  double  c2) 

{ 

Cl  =  cl; 

C2  -  c2; 

} 


/*★********★******************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


:  EnableLinearFitting (int  SonarNumber) 

:  int  SonarNumber 

:  It  causes  the  background  system  to  gather  data  points  from 
:  sonar  SonarNumber  and  form  them  into  line  segments  as  governed 
:  by  the  linear  fitting  algorithm. 

:  None 
:  None 
:  None 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


****************************************************************************** 


/ 


void 

EnableLinearFitting ( int  SonarNumber ) 

{ 

sonar__table [SonarNumber]  .fitting  ■  1;  /*  sonar_table  is  global  variable 

which  is  defined  in  sonar. c  the 
data  structure  of  sonar_table  is 
SONARD  which  is  defined  in  sonar. h  */ 

} 


/************************************************************************** ***** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


DisableLinearFitting (int  SonarNumber) 
int  SonarNumber 

It  causes  background  system  to  cease  forming  line  segments  for 
sonar  SonarNumber . 

None 

GenerateSegment ()  in  sonarmath.h. 

None 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 
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*******************************************************************************y 


void 

DisableLinearFitting (int  SonarNumber) 

{ 

GenerateSegment (SonarNumber) ; 
sonar_table [SonarNumber] . fitting  =  0; 

} 

/****************************************************************************** 
FUNCTION  :  LinearFitting (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  controls  the  fitting  of  point  data  to  straight  line  segments. 

:  First,  it  tests  if  the  new  coming  point  is  not  far  from  the 
:  fitted  line.  If  the  test  is  passed,  the  point  is  added  to  test 
:  if  the  thinness  test  is  passed.  If  it  is  passed,  the  addition  is 
:  finalized.  If  any  of  the  tests  fail,  the  line  segment  is  ended 
:  and  a  new  one  started.  The  completed  line  segment  is  stored  in  a 
:  data  structure  called  SEGMENT,  and  segments  are  linked  together 
:  in  a  linked  list. 

RETURNS  :  None 

CALLS  :  GenerateSegment ( )  in  sonarmath.h 

:  Disable Interrupts ()  in  spare. c 

:  Enablelnterrupts ( )  in  spare. c 

:  AddToSegment ( )  in  sonarmath.c  *Local  Prototypes  in  sonarmath.c* 
CALLED  BY  :  SonarSysControl ( )  in  sonar. h 

COMMENTS  :  Revised  by  Y.  Kanayama, 07-07-93 

:  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 
:  Disablelnterrupts ()  &  Enablelnterrupts ( )  added  by 
:  Mahmoud  wahdan  on  Aug  16,95  to  solve  the  problem  of 
:  some  numbers  becomes  very  big  or  very  somall  (wrong  calculation) . 

*********************************** ** *** *** *****  * *** ***** *** ** ** ************** y 

void 

LinearFitting (int  SonarNumber) 

{ 

double  sonar_range; 

double  sonar_rangeO; 

POINT  p; 

double  alpha,  r,  delta; 

Disablelnterrupts ( ) ; 

sonar_range  =  sonar_table [SonarNumber] .d; 
sonar__rangeO  =  sonar_table  [SonarNumber]  .d0; 

if  (sonar_jrange  <  9.3  M  sonar_range  >  409.0) 

{ 

GenerateSegment (SonarNumber) ; 

Enablelnterrupts ( ) ; 
return; 

} 

p  =  sonar_table [SonarNumber] .global; 

if  (fabs (sonar_range  -  sonar  rangeO)  >3.0  ) 

{ 

GenerateSegment (SonarNumber) ; 

AddToSegment (SonarNumber,  p) ; 

Enablelnterrupts ( ) ; 
return; 

} 

if  (segment_data [SonarNumber] .rnOO  <  1.5) 

{ 

AddToSegment (SonarNumber,  p) ; 

Enablelnterrupts ( ) ; 
return; 

} 

r  -  segment_data [SonarNumber] .seg.r; 
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alpha  =  segment_data [SonarNumber] .seg. alpha; 

delta  =  f abs ( r  -  p.X  *  cos (alpha)  -  p.Y  *  sin (alpha)}; 

if  (delta  >  MAXVAL(C2,  Cl  *  sonar_range) ) 
GenerateSegment (SonarNumber) ; 

AddToSegment (SonarNumber,  p) ; 

Enablelnterrupts ( ) ; 
return; 


} 


/*  End  linear_fitting  */ 


/★a**************************************************************************** 


FUNCTION  :  AddToSegment (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 


PURPOSE 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


POINT  p 

It  calculates  new  interim  data  for  the  line  segment  and  stores 
it  in  segment_data [ SonarNumber ] . 

It  also  changes  the  end  point  values  to  the  point  being  added. 
None 

LinearFitting ( )  in  sonarmath.h. 

SonarSysControl { )  in  sonar. h. 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


★*************************★★*********★*******************★***★****************/ 


void 

AddToSegment (int  SonarNumber,  POINT  p) 
{ 


double  mOO,  mlO,  mOl,  m20,  mil,  m02; 

double  alpha,  r; 

double  mux,  muy,  mm20,mmll,  mm02; 

double  x,  y; 


mOO  =  segment_data [SonarNumber] .mOO  +=  1.0; 
mlO  =  segment_data [SonarNumber] .ml 0  +=  p.X; 
mOl  =  segment_data [SonarNumber] .mOl  +=  p.Y; 
m2 0  =  segment_data [SonarNumber] .m20  +=  SQR(p.X); 
mil  =  segment_data [SonarNumber] .mil  +=  p.X  *  p.Y; 
m02  =  segment_data [SonarNumber] .m02  +=  SQR(p.Y); 

if  (mOO  <  1.5) 

segment_data [ SonarNumber ] .seg. start  =  p; 

mux  -  mlO  /  mOO; 

muy  =  mOl  /  mOO; 

mm20  -  m20  -  SQR(mlO)  /  mOO; 

mmll  =  mil  -  mlO  *  mOl  /  mOO; 

mm02  =  m02  -  SQR(mOl)  /  mOO; 

y  -  -2.0  *  mmll; 

x  =  mra02  -  mm20; 


if  (mOO  >  1.5)  { 

if  (y  ==  0.0  &&  x  ==  0,0)  /*  This  test  is  put  because  when  */ 

/*  atan2(0,0)  the  system  crashes  */ 
alpha  =0.0;  /*  July  24th  95  by  mahmoud  wahdan  */ 

else 

alpha  =  atan2(y,  x)  /  2.0; 


r  =  mux  *  cos (alpha)  +  muy  *  sin(alpha); 

segment_data[ SonarNumber] .seg. alpha  =  alpha; 
segment_data [SonarNumber] .seg. r  =  r; 
segment_data [SonarNumber] .seg. end  =  p; 
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} 


} 


/******★******★***************************************★******•***★************** 
FUNCTION  :  GenerateSegment (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  completes  segments  at  the  end  of  a  data  run.  Necessary 

:  because  the  linear  fitting  function  only  terminates  a  segment 
:  based  on  the  data  and  it  has  no  way  of  knowing  that  the  user 
:  has  stopped  collecting  data. 

RETURNS  :  None 

CALLS  :  EndSegmentO  in  sonarmath.h. 

:  BuildListO  in  sonarmath.c  *Local  Prototypes  in  sonarmath.c* 

:  ResetMoments ( )  in  sonarmath.c  *Local  Prototypes  in  sonarmath.c* 
CALLED  BY  :  DisableLinearFitting ( )  in  sonarmath.h. 

:  LinearFitting ( )  in  sonarmath.h. 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

*********************************************************************** *******y 

void 

GenerateSegment (int  SonarNumber) 

{ 

SEGMENT_RES  *seg_ptr; 


if  (segment_data [SonarNumber] .mOO  >  10.0) 

{ 

seg_ptr  =  Ends egment (SonarNumber) ; 

BuildList  (seg_j?tr,  SonarNumber); 

} 

ResetMoments (SonarNumber) ; 

} 

/************* ***************************************************************** 
FUNCTION  :  Ends egment (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  allocates  memory  for  the  segment  data  structure,  loads  the 

:  correct  values  into  it  and  returns  a  pointer  to  the  structure. 

RETURNS  :  a  pointer  to  the  SEGMENT  structure 

CALLS  :  None 

CALLED  BY  :  None 

COMMENTS  :  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 

:  and  data  structure  LINE-SEG  is  removed  and  added  to 
:  SEGMENTJRES.  Refer  to  this  data  structure  in  sonar. h 
************************** **************************************************** ^ 

SEGMENT_RES  * 

EndSegment (int  SonarNumber) 

{ 

SEGMENT_RES  tmpSeg; 

SEGMENT_RES  *seg_ptr; 

double  length; 

double  delta; 

seg__ptr  =  &segstruct; 

tmpSeg  =  segment_data [SonarNumber] . seg; 

delta  -  tmpSeg. start .X  *  cos (tmpSeg. alpha)  + 

tmpSeg. start. Y  *  sin (tmpSeg. alpha)  -  tmpSeg. r; 

tmpSeg. s tart. X  -=  delta  *  cos (tmpSeg. alpha ) ; 
tmpSeg. start. Y  -=  delta  *  sin (tmpSeg. alpha ) ; 
delta  =  tmpSeg. end. X  *  cos (tmpSeg. alpha)  + 

tmpSeg. end. Y  *  sin (tmpSeg. alpha)  -  tmpSeg. r; 

tmpSeg. end. X  -=  delta  *  cos (tmpSeg. alpha) ; 
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tmpSeg.end.Y  -=  delta  *  sin (tmpSeg. alpha) ; 
length  =  sqrt (SQR{ tmpSeg. start .X  -  tmpSeg. end. X)  + 
SQR ( tmpSeg. s tart. Y  -  tmpSeg.end.Y)); 


seg_ptr->start .X 

seg_j)tr->  start .  Y 

seg_ptr->end.X 

segjptr->end.Y 

se9_ptr->alpha 

segjptr->r 

seg_ptr->length 

seg_ptr->SonarNumber 

segjptr->mOO 


tmpSeg. start. X; 
tmpSeg. start. Y; 
tmpSeg. end. X; 
tmpSeg . end . Y; 
tmpSeg. alpha; 
tmpSeg. r; 
length; 

SonarNumber; 

segment_data  [SonarNumber] .mOO; 


return  segjptr; 


} 


/****************  ************************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


:  BuildList (SEGMENT_RES  *ptr,  int  SonarNumber) 

:  int  SonarNumber 

:  SEGMENT_RES  *ptr 

:  It  accepts  a  pointer  to  a  segment  data  structure  and  a  sonar 
:  number,  and  appends  the  segment  structure  to  the  tail  of  a 
:  linked  list  of  structures  for  that  sonar. 

:  None 

;  LogSonarSegmentData ( )  in  sonarlog.h. 

:  GenerateSegment ( )  in  sonarmath.h. 

:  Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


*********************************************** ******************************* 


/ 


void 

BuildList (SEGMENT_RES  *ptr,  int  SonarNumber) 

{ 


int  next; 

next  =  (SegListTail [SonarNumber]  <  99)  ?  ++SegListTail [SonarNumber]  :  0; 
seg_JList  [SonarNumber]  [next]  =  *ptr; 

LogSonarSegmentData (SonarNumber,  seg_list [SonarNumber] [next] ) ; 
LogSonarSegmentDataAr ray (SonarNumber,  seg_list [SonarNumber] [next] ) ; 


/************************************************** **************************** 


FUNCTION  :  ResetMoments (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  resets  the  accumulative  values  in  segment_dat a [SonarNumber] 

:  (mOO,  mlO,  mOl,  m20,  mil,  m02)  to  zero. 


RETURNS 
CALLS 
CALLED  BY 

COMMENTS 


None 

None 

InitSonarmath ( )  in  sonarmath.h. 

GenerateSegment ( )  in  sonarmath.h. 

Cleaned  and  documented  by  Mahmoud  Wahdan  on  3-7-95 


********************************************** ******************************** 


/ 


void 

ResetMoments (int  SonarNumber) 

{ 

segment_data [SonarNumber]  .mOO  =  0.0 
segment_data [SonarNumber] .mlO  =  0.0 
segment_data [SonarNumber] .mOl  =  0.0 
segment_data [SonarNumber] .m2 0  =  0.0 
segment_data [SonarNumber] .mil  =0.0 
segment_data [SonarNumber] .m02  =0.0 

} 


y************************************************************************^.^^^^^,^ 
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FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 

CALLS 
CALLED  BY 
COMMENTS 


:  GetLastSegment (int  SonarNumber) 

:  int  SonarNumber 

:  It  returns  a  pointer  to  the  latest  segment  on  the  linked  list 
:  of  segments  for  sonar. 

:  If  GetLastSegment  is  called  on  an  empty  list  a  null  pointer  is 
:  returned. 

:  returns  the  pointer  to  the  last  segment  on  the  linked  list  of 
:  segments  for  sonar.  NULL  if  there  is  empty  list 
:  None 
:  None 

:  Written  by  Mahmoud  Wahdan  on  7-3-95. 


SEGMENT_RES  * 

GetLastSegment (int  SonarNumber) 

{ 

SEGMENT_RES  *Last_seg; 
int  index; 


index  =  SegListTail [SonarNumber] ; 
if  (index  ==  -1) 


Last_seg  =  NULL; 


else 

Last_seg  =  &seg_list [SonarNumber] [index] ; 
return  Last_seg; 


/★****★•*•*★***•*** 


****************★*★★****★★**★•*★****★*★★*★***★**••*•★****•★*★•*■**★★** 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


GetCurrentSegment (int  SonarNumber) 
int  SonarNumber 

It  returns  a  pointer  to  the  segment  currently  under  construction 
if  there  is  one,  otherwise,  returns  null  pointer.  This  is 
accomplished  by  calling  EndSegment,  copying  the  data  into 
segstruct  and  then  returning  a  pointer  to  segstruct.  The  memory 
allocated  by  EndSegment  is  then  freed. 

returns  the  pointer  to  the  current  segment  under  construction. 

None 

None 

Written  by  Mahmoud  Wahdan  on  7-3-95. 


SEGMENT__RES  * 

GetCurrentSegment (int  SonarNumber) 

{ 

SEGMENT_R£S  *segj?tr; 

seg_ptr  -  EndSegment (SonarNumber) ; 
return  seg_ptr; 


} 

/********★****★***★***  source  Code  For  Localization  Correction  ****************/ 


/★*********★*★*★**★★**★**★*****★★******★*****★****★•*•********★*****  ************** 
FUNCTION  :  Ge t Segment Con fig ( ) 

PARAMETERS  :  None 

PURPOSE  :  It  returns  a  configuration  after  applied  the  LF.  It  is  used  in 

the  localization  correction. 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


returns  a  configuration. 

None 

None 

Written  by  Mahmoud  Wahdan  on  10-19-95. 


CONFIGURATION 
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GetSegmentConf ig ( ) 


CONFIGURATION  qsegment; 

double  x,  y; 

Disablelnterrupts { ) ; 

qsegment. Posit. X  -  seg_end_config->end.X; 
qsegment. Posit. Y  =  seg_end_config->end. Y; 

x  =  seg__end_config->start.X  -  seg_end__config->end.X; 

y  =  seg_end_config->start. Y  -  seg_end_config->end.  Y; 


if  (y  ==  0.0  &&  x  ==  0.0) 


qsegment. Theta  =  0.0; 


else 


qsegment. Theta  =  atan2(y,  x) ; 

qsegment .Kappa  =  0.0; 
Enablelnterrupts () ; 

return  qsegment; 


} 


/****************************************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


:  WaitSegment (int  SonarNumber) 

:  int  SonarNumber 

:  busy  wait  until  the  line  segment  being  built  is  completed. 

It  is  used  in  localization  correction. 

:  void 
:  None 
:  None 

:  Written  by  Mahmoud  Wahdan  on  10-19-95. 


*******************************************************************************y 

void 


WaitSegment (int  SonarNumber) 


int  index; 

SEGMENT_RES  *seg; 

index  -  SegListTail [SonarNumber] ; 


while (index  =-  SegListTail [SonarNumber] )  { 

7 

> 

seg  =  &seg_list [SonarNumber] [SegListTail [SonarNumber] ] ; 
seg_end_config  -  &seg_list [SonarNumber] [SegListTail [SonarNumber]]; 


> 


/**★****************************★*** ************************* ******************* 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 

CALLS 


Match ( ) 

CONFIGURATION  :  qnew_seg 
CONFIGURATION  :  qmodel_seg 

compare  between  new  segment  and  model  wall  segment, 
if  match  then  call  localization  correction  routine. 
It  is  used  in  localization  correction, 
void 

WaitSegment ( )  in  sonarmath.c 
setRobotConfigO  in  seqcmd.c 
compose ()  in  geometry. c 
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in  geometry. c 


:  inverse () 

CALLED  BY  :  none 

COMMENTS  :  written  by  Mahmoud  Wahdan  on  Nov.  4,  95 

****************************** ************************************************* f 
/* 

Match (CONFIGURATION  qnew_seq,  CONFIGURATION  qmodel_seq) 

double  length  =  50.0; 
double  orintation  =  2.0; 
double  delta_dist; 
double  delta_theta; 

Disablelnterrupts { ) ; 

delta_dist  =  eu_dis (qnew_seq. Posit .X,  qnew_seq. Posit . Y, 

qmodel_seq. Posit. X,  qmodel_seq. Posit. Y) ; 
delta_dist  =  sqrt  ( (qnew_seq.  Posit  .X  -  qmodel_seq. Posit .X)  * 

(qnew_seq. Posit. X  -  qmodel_seq. Posit .X)  + 

(qnew_seq. Posit .Y  -  qmodel_seq. Posit. Y)  * 

(qnew_seq. Posit. Y  -  qmodel_seq. Posit .Y) ) ; 

delta_theta  =  (qnew_seq. Theta  -  (qmodel_seq.Theta/RAD) ) ; 

if ( (delta_dist  <  length/2.0)  &&  (delta_theta  <  (orintation/RAD) ) ) 
return  -1; 

else 

return  0; 

} 

*/ 

/**★****************************************** ********************************** 
FUNCTION  :  CorrectOdometryError { ) 

PARAMETERS  :  CONFIGURATION  :  qsonar 

:  CONFIGURATION  :  qmodel 

PURPOSE  :  correct  the  error  if  there  is  a  difference  between  where 

:  the  vehicle  thinks  it  is  and  where  the  vehicle  really  is. 
RETURNS  :  void 

CALLS  :  getRobotConfig ( )  in  motion. c 

:  setRobotConfig ( )  in  seqcmd.c 

:  compose ()  in  geometry. c 

:  inverse ()  in  geometry. c 

CALLED  BY  :  none 

COMMENTS  ;  written  by  Mahmoud  Wahdan  on  Jul.  30,  95 

********************************************** *********************************/ 

/* 

void 

CorrectOdometryError (CONFIGURATION  qsegment,  CONFIGURATION  qmodel) 

CONFIGURATION  qodometry; 

CONFIGURATION  qsegment_inverse; 

CONFIGURATION  qactual; 

CONFIGURATION  epsilon; 

*/ 

/***************************************************** *^ 

/*  qacual  =  qmodel  o  qsegment__inverse  o  qodometry  */ 

/*  =  epsilon  o  qodometry  */ 

/*  epsilon  =  qmodel  o  qsegment_inverse  */ 

/******************************************************  . 

I* 

qodometry  =  getRobotConfig () ; 

qsegment_inverse  =  inverse (qsegment) ; 

epsilon  =  compose (&qmodel,  &qsegment_inverse) ; 

qactual  -  compose ( &epsilon,  & qodometry ) ; 

setRobotConfiglmm (qactual) ; 

#ifdef  LOCALIZATION 

IOprintf (localizationlO, "%f  %f  %f\n\n%f  %f  %f\n\n%f  %f  %f\n\n%f  %f  %f\n\n", 
epsilon. Posit. X, 
epsilon. Posit. Y, 
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epsilon. Theta, 

qsegment . Posit .X, 
qsegment . Posit . Y , 
qsegment. Theta, 

qodometry . Posit .X, 
qodometry . Posit . Y, 
qodome  t  r y . The t a , 

qactual . Posit .X, 
qactual . Posit . Y, 
qactual. Theta 
) ; 


#endif 

> 

*/ 


/*******************  source  Code  For  Linear  Fitting  with  Decay  ****************/ 
/*  ========“======-“=====™™=====-========  */ 


/***★*★********************** ************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


EnableLinearFittingDecay (int  SonarNumber) 
int  SonarNumber 

It  causes  the  background  system  to  gather  data  points  from 
sonar  SonarNumber  and  form  them  into  a  continuously  updated 
line  segments  as  governed  by  the  linear  fitting,  with 
decay  algorithm. 

None 

None 

None 

Added  by  Dan  Wells  on  04-15-99 


**********************************************************  *  *  *  **  ***  *  *  *  *  *  *  * * * *  *  * 


/ 


void 

EnableLinearFittingDecay (int  SonarNumber ) 

{ 

sonar_table [SonarNumber] .decayFitting  =  1;  /*  sonar_table  is  global  variable 

which  is  defined  in  sonar. c.  the 
data  structure  of  sonar_table  is 
SONARD  which  is  defined  in  sonar. h  */ 


/a*************************************************************************** *** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 
CALLS 
CALLED  BY 
COMMENTS 


:  DisableLinearFittingDecay (int  SonarNumber) 

:  int  SonarNumber 

:  It  causes  background  system  to  cease  linear  fitting  with  decay 
:  for  sonar  SonarNumber. 

:  None 
:  None 
:  None 

:  Added  by  Dan  Wells  on  04-15-99 


***** ***************************************************** ********************* j 


void 

DisableLinearFittingDecay (int  SonarNumber) 

{ 

sonar_t able [SonarNumber] .decayFitting  =  0; 

\ 


/******************************************************************* *********** 
FUNCTION  :  Re setMoments Decay (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  resets  the  values  in  seg_data_decay [SonarNumber] . 

:  Effectively  restarts  the  linear  fitting  with  decay  algorithm. 
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:  A  static  variable  is  used  to  test  for  the  initial  call  and 
:  if  so,  sets  the  decay  factor  to  the  default,  otherwise 
:  leaves  this  field  unchanged.  This  allows  the  user  to 
:  set  the  decay  factor  through  the  SetDecayFactor ( )  function.  - 
RETURNS  :  None 

CALLS  :  None 

CALLED  BY  :  InitSonarmath ( )  in  sonarmath.h. 

COMMENTS  :  Added  by  Dan  Wells  on  04-15-99. 

****************** ****************************************************** *******/ 

void 

ResetMomentsDecay (int  SonarNumber) 

{ 

static  int  firstTimeCalled  =  1; 

seg_data_decay [SonarNumber] .SonarNumber  -  SonarNumber; 
seg_data_decay [SonarNumber] .usable  =  0; 
seg_data_decay [ SonarNumber] .numPoints  =  0.0; 
seg_data_decay[ SonarNumber] .alpha  =  PI/2.0; 
seg_data_decay [SonarNumber] .r  =  0.0; 
if  (firstTimeCalled)  { 

segjdata_decay  [SonarNumber] .decayFactor  *  DEFAULTJDECAY; 
firstTimeCalled  =  0; 

} 

seg_data_decay [SonarNumber] .m00  =  0.0; 
seg_data_decay [SonarNumber] .ml  0  =  0.0; 
seg_data_decay [SonarNumber] .rnOl  =  0.0; 
seg_data_decay [SonarNumber] .mil  =  0.0; 
seg_data_decay [SonarNumber]  .m20  -  0.0; 
seg__data_decay [SonarNumber]  .m02  -  0.0; 


/****★***********************★★*★★***********★***■**★***★★★★**■**★********★*★*★** 
FUNCTION  :  LinearFittingDecay (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  controls  the  fitting  of  point  data  to  the  current  linear 

:  fitted  line  using  a  decay  factor  for  previously  entered 
:  points . 

RETURNS  :  None 

CALLS  ;  Disablelnterrupts 0  in  spare. c 

:  Enablelnterrupts ( )  in  spare. c 

:  AddToSegmentDecay ( )  in  sonarmath.c  *Local  Prototypes  in  sonarmath.c* 
CALLED  BY  :  SonarSysControl ( )  in  sonar. h 

COMMENTS  :  Added  by  Dan  Wells  on  04-15-99 

:  Disablelnterrupts ()  &  Enablelnterrupts ()  added  to  solve  the 
:  problem  of  some  numbers  becomes  very  big  or  very 
:  small  (wrong  calculation) . 

★★★★★★a***********************************************************************/ 

void 

LinearFittingDecay ( int  SonarNumbe r ) 

{ 

double  sonar_range; 

double  sonar_range0; 

POINT  p; 

Disablelnterrupts () ; 

sonar_range  -  sonar_table [SonarNumber] .d; 
sonar_range0  =  sonar_table [SonarNumber] .d0; 

if  (sonar_range  <  9.3  ||  sonar_range  >  80)  /****409 .0) *****/ 

{ 

Enablelnterrupts () ; 
return; 

} 

p  =  sonar_table [SonarNumber] .global; 

AddToSegmentDecay (SonarNumber,  p) ; 
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Enablelnterrupts ( ) ; 
return; 

} 

/****************************************************************** ************ 
FUNCTION  :  AddToSegmentDecay {int  SonarNumber,  POINT  p) 

PARAMETERS  :  int  SonarNumber 

:  POINT  p 

PURPOSE  :  It  calculates  new  line  segment  using  linear  fitting  with  decay. 

RETURNS  :  None 

CALLS  :  None 

CALLED  BY  :  LinearFittingDecay ( )  in  sonarmath.h. 

COMMENTS  • :  Added  by  Dan  Wells  04-15-99. 

******* ***********************************************************************y 
void  , 

AddToSegmentDecay (int  SonarNumber,  POINT  p) 

{ 

double  decay  =  seg_data__decay  [SonarNumber]  .decayFaCtor; 

double  mOO,  mlO,  mOl,  m20,  mil,  m02; 

double  alpha,  r; 

double  mux,  muy,  M2 0, Mil,  M02; 

double  x,  y; 


mOO  =  seg__data_decay  [SonarNumber]  .mOO; 
mlO  =  seg_data_decay [SonarNumber] .ml 0; 
mOl  *  seg_data_decay [SonarNumber] .mOl; 
m20  =  seg_data_de cay [SonarNumber] .m20; 
mil  =  seg_data__de cay  [SonarNumber]  .mil; 
m02  *  seg_data_decay [SonarNumber] .m02; 

mOO  =  mOO  *  decay  +  1.0; 

mlO  =  mlO  *  decay  +  p.X; 

mOl  =  mOl  *  decay  +  p.Y; 

mil  =  mil  *  decay  +  p.X  *  p.Y; 

m20  =  m20  *  decay  +  p.X  *  p.X; 

m02  -  m02  *  decay  +  p.Y  *  p.Y; 

mux  =  mlO  /  mOO; 

muy  =  mOl  /  mOO; 

Mil  -  mil  -  (mlO  *  mOl)  /  mOO; 

M20  =  m20  -  SQR(mlO)  /  mOO; 

M02  =  m02  -  SQR(mOl)  /  mOO; 

seg_data_decay [SonarNumber] .mOO  =  mOO; 
seg_data_decay[ SonarNumber] .mlO  =  mlO; 
seg_data_de cay [SonarNumber]  .mOl  =  mOl; 
seg_data_decay [SonarNumber] .mil  =  mil; 
seg_data_decay [SonarNumber]  ,m20  =  m20; 
seg_data_decay [SonarNumber] .m02  =  m02; 

seg_data_decay [ SonarNumber] .numPoints  +=  1.0; 

if  ( seg_data_decay[ SonarNumber ] .numPoints  >  1.5)  {  /*  Need  2  data  */ 

/*  for  a  line  */ 

seg_data_decay [SonarNumber] .usable  -  1; 
x  =  M02  -  M20; 
y  =  -2.0  *  Mil; 

if  ((x  ==  0.0)  &&  (y  ==  0.0))  { 

alpha  «  0.0; 

} 

else  { 

alpha  -  atan2(y,  x)  /  2.0; 

} 

r  =  mux  *  cos (alpha)  +  muy  *  sin(alpha); 

seg_data_decay[ SonarNumber] .alpha  =  alpha; 
seg_data_decay [SonarNumber] .r  -  r; 
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} 


/**★**************************★***★****★*★**★*********************************** 
FUNCTION  :  GetSegmentDecay (int  SonarNumber) 

PARAMETERS  :  int  SonarNumber 

PURPOSE  :  It  returns  a  pointer  to  linear  fitted  with  decay  segement 

:  for  the  specified  sonar. 

RETURNS  :  returns  the  pointer 

CALLS  :  None 

CALLED  BY  :  None 

COMMENTS  :  Added  by  Dan  Wells  on  04-15-99 

★*****■*•*★★*★***★**  ★*★******★★*★*****************************.**.**.*.*.*****.***.***,*,*  j 

SEGMENT_RES_DECAY 
GetSegmentDecay (int  SonarNumber) 

{ 

'  SEGMENTJRES_DECAY  tempSegDecay; 

tempSegDecay  -  seg_data_decay [SonarNumber] ; 
return  tempSegDecay; 


/★I**************************************************.*.*.***********.**.**.*,****.***.*** 

FUNCTION  :  SetDecayFactor (int  SonarNumber,  double  decay) 

PARAMETERS  :  int  SonarNumber 

:  double  decay 

PURPOSE  :  It  sets  the  decay  factor  for  linear  fitted  with  decay 

:  algorithm  for  the  specified  sonar.  It  checks  decay  value 
;  between  0  and  1  and  sets  to  default  if  out  of  bounds. 

RETURNS  :  void 

CALLS  :  None 

CALLED  BY  :  None 

COMMENTS  :  Added  by  Dan  Wells  on  04-22-99 

I'*****************-*************************'*********-***************************  j 

void 

SetDecayFactor (int  SonarNumber,  double  decay) 

{ 

if  (  (decay  <  0.0)  ||  (decay  >  1.0)  )  { 
printf (n\nDecay  set  to  default"); 
decay  =  DEFAULT  JDECAY ; 

} 

seg_data_decay [SonarNumber] .decayFactor  -  decay; 

} 
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APPENDIX  B:  USER  FILE  AND  NEW  WALL  COMMAND  FILE 


S**’************************************************************************-#*** 
File  name:  user.c 

Descriptions:  set  up  system  with  initial  parameters  and  motion  mode,  then 
check  for  end  of  run  conditions 
Revision  history: 

*******************************************************  *********************** / 

#include  "user.h" 

#include  "seqcmd.h" 

#include  "wallcmd.h" 
tinclude  "math.h" 

#include  "constants .h" 

#include  "queue. h" 
finclude  "geometry.h" 

I************************************************************************,,'**!'* 
Function  :  user ( ) 

Purpose  :  Makes  a  complete  circuit  (returns  to  start  point) 

:  using  trackWall  motion  mode 
Parameters :  void 
Returns  :  void 
Comments  :  Dan  Wells  5/99 

****************************************************************************** 
void  user ( ) 

{ 

CONFIGURATION  startConfig,  currentConfig; 
int  sonarNum,  wallSide; 
double  sigma,  decay,  offset; 
int  done,  logSonar; 

/*  Get  user  input  and  set  appropriate  parameters  */ 

print f ("\nEnter  1  to  log  sonar  data,  0  otherwise  :  n); 
logSonar  =  Getlnt ( ) ; 

printf ("\nlnput  desired  offset  from  center  of  robot  to  wall  (cm)  :  "); 
offset  =  GetRealO; 

printf ("\nlnput  desired  smoothness  (cm)  e.g.  20  :  "); 
sigma  *  GetReal { ) ; 

setSigmalmm( sigma) ; 

printf ("\nlnput  decay  factor  (e.g.  0.9)  :  "); 
decay  -  GetReal ( )  ; 

printf ("\nEnter  0  for  wall  on  RIGHT  side"); 
printf ("\nEnter  1  for  wall  on  LEFT  side") ; 
printf ( " \nChoice>  " ) ; 
wallSide  *  Getlnt ( ) ; 

/*  Turn  on  motion  logging  */ 

MotionLog(0, 50, 0) ; 

/*  Set  up  sonar  number  depending  on  wall  following  side  */ 
switch  (wallSide)  { 
case  0: 

sonarNum  =  S090; 
break; 
case  1: 

sonarNum  =  S270; 
break; 
default: 

printf ("\nERROR:  Illegal  wall  selection  in  user.c"); 
exit (1) ; 
break; 
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} 


/*  Set  the  linear  fitting  decay  factor  for  appropriate  sonar  */ 
SetDecayFactor (sonarNum,  decay); 

/*  Turn  on  sonar  logging  if  user  requested  */ 
if  (logSonar  ==  1)  { 

InitSonarlog ( ) ; 

SonarLog(10,  0,  sonarNum,  SONAR_GLOBAL) ; 

} 

/*  This  sets  up  the  wall  following  motion  mode  */ 
trackWall ( sonarNum,  offset ) ; 

/*  Save  starting  configuration  for  later  comparison  */ 
startConfig  =  getRobotConfigO ; 

/*  Let  robot  start  to  move  (lets  it  get  outside  the  100  cm  */ 

/*  of  the  starting  point)  */ 
while  (getTotalDistanceImm( )  <  100)  { 
waitMS  (50) ; 

/*  do  nothing  */ 

> 

/*  Now  check  if  robot  has  halted  or  gone  around  the  circuit  */ 

/*  Checking  the  halt  condition  allows  data  download  if  robot  */ 

/*  halts  before  complete  circuit  is  made  */ 
done  =  0; 

while  (  (VelocityLinear ( )  >0.0)  &&  (!done)  )  { 
waitMS  (50) ; 

currentConfig  =  getRobotConfigO; 

/*  stop  if  within  100  cm  and  30  degrees  of  starting  position  */ 
if  (  (distance (startConfig. Posit, currentConfig. Posit)  <  100)  && 

(fabs (norm (startConfig. Theta  -  currentConf ig. Theta) )  <  30*d2r)  )  { 
done  =  1; 

} 

} 

/*  Disable  everything  */ 

DisableLinearFittingDecay ( sonarNum) ; 

DisableSonar (sonarNum) ; 

DisableSonar (S000) ; 

/*  Stop  robot  */ 
stopImmO  ; 

return;  /*  Returns  to  main  for  data  download  * / 


/★♦★★★a************************************************************************ 

File  name:  wallcmd.c 

Descriptions:  set  up  the  currentPath  variable  in  motion. c  for  wall  following 
mode 

Revision  history: 


#include 
tinclude 
#include 
#include 
# include 
#include 
#include 
#include 


"definitions. h" 
"motion. h" 
"stdiosys.h" 
<math.h> 
"geometry. h" 
"sonar ,h" 
"sonarcard.h" 
"sonarmath.h" 
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/****************************************************************************** 
FUNCTION :  trackWall ( ) 

PARAMETERS:  sonarNum  -  specifies  left/right  wall  by  sonar  number  S270/S090 
offset  -  specifies  offset  distance  magnitude  from  wall 
PURPOSE:  sets  up  the  currentPath  LINE  variable  in  motion. c  for  wall 

following 
RETURNS :  void 

CALLED  BY:  user() 

CALLS :  setCurrentPath ( ) f  setRobotConf iglmm ( ) 

COMMENTS : 

**************************** ************************************************** / 
void 

trackWall (int  sonarNum,  double  offset) 

{ 

double  k; 

LINE  pathElement; 

double  sigma  =  sigmaValue { ) ; 

/*  sonarNum  determines  side  of  wall  following  */ 

/*  Constants  defined  in  sonar. h  */ 

/*  S090  -  wall  on  right  side,  S270  -  wall  on  left  side  */ 
switch  (sonarNum)  { 
case  S090: 

pathElement . Type  =  TRACKRWALLMODE; 

offset  -  offset  <  f abs ( sonar_table [ S090 ] . SonarPosit . Y ) +MIN_EDGE  OFFSET  ? 
DEFAULT_WALL_OFFSET  :  offset; 

break; 
case  S270: 

pathElement. Type  =  TRACKLWALLMODE ; 

offset  -  offset  <  fabs (sonar_table [S270] .SonarPosit .Y) +MIN_EDGE  OFFSET  ? 
-DEFAULT_WALL_OFFSET  :  -offset; 

break; 

default: 

printf ("\nERROR:  illegal  value  for  sonarNum  in  trackWall ()”) ; 

exit (1) ; 

break; 

} 


pathElement. config  =  defineConfig (0 . 0,  0.0,  0.0,  0.0); 

k  =  1.0/sigma  ; 
pathElement . a  =  3.0*k; 
pathElement .b  -  3.0*k*k; 
pathElement .c  =  k*k*k; 

pathElement. Radius  -  offset;  /*  won»t  initially  use  Radius  in  wall  following 

so  we  use  it  to  pass  in  the  requested 
wall  offset  */ 

pathElement. Center .X  =  0.0; 
pathElement. Center .Y  =  0.0; 

setPathElement (pathElement) ;  /*  defined  in  motion. c  -  sets  currentPath  */ 
setRobotConf iglmm (pathElement. config) ;  /*  matches  robot  with  currentPath  */ 

EnableSonar (S000) ; 

EnableSonar (sonarNum)  ; 

EnableLinearFittingDecay ( sonarNum) ; 

return; 

} 
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APPENDIX  C:  THE  WALLHUGRULE  FUNCTION 
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/*********************** ****************************************************** 
Function  :  wallHugRule ( ) 

Purpose  :  To  determine  the  linear  and  rotational  velocities  needed  to  put 
or  keep  Yamabico  at  the  correct  offset  from  the  wall. 

Parameters:  VELOCITY  actual,  commanded 

Returns  :  The  required  linear  velocity,  rotational  velocity 
Comments : 

***** ************************************************************ ************ i 

static  VELOCITY 

wallHugRule (VELOCITY  actual,  VELOCITY  commanded) 

{ 

CONFIGURATION  curWall,  offsetConfig,  relativeConfig,  inverseConfig, 
tempConfig; 

SEGMENT_RES_DECAY  curSeg; 
double  wallx,  wally,  wallTheta; 
static  double  prevWallTheta=0.0; 

static  double  initCircleTheta; 

double  deltaDi stance; 
double  deltaAngle; 

static  double  savedSigma;  /*  holds  old  sigma  value  while  in  turn  */ 
static  int  pingedOnce  =0, 
pingedTwice  =  0; 

static  int  firstTimeCalled  =  1;  /* only  true  if  first  time  thru  wallHugRule*/ 
int  sonarNum; 

double  distOOO,  sideDist,  lastSideDist;  /*  sonar  distances  */ 

static  double  offset;  /*  offset  is  stored  in  the  initially  unused  */ 

/*  (for  this  mode)  currentPath. Radius  field  */ 

/*  offset  <  0  for  left  wall  following  */ 

/*  offset  >  0  for  right  wall  following  */ 

/*  flags  to  control  motion  calculation  */ 

static  int  halted  =  0;  /*  indicates  motion  halted  (collision)  */ 

static  int  inTurn  =  0;  /*  designates  when  robot  in  a  turn  */ 

static  int  inTransition  =  0;  /*  within  transition  area  but  still  */ 

/*  waiting  for  usable  Unfit  data  */ 
static  int  turnType  =  0;  /*  0=inside  turn,  l=outside  turn  */ 

/*  2=on  circle  */ 

int  trackType;  /*  0=wall  follow,  l=track  currentPath  */ 

double  jL;  /*  Steering  function  value  */ 

double  kk,  kk2;  /*  kappa,  kappa  squared  */ 
double  k;  /*  steering  gain  parameter  */ 

/*  Set  sonarNum,  turn  angle,  and  offset  for  left  or  right  wall  following  */ 
/*  Note:  turn  angle  (deltaAngle)  is  set  for  inside  turns,  use  negative  */ 
/*  of  this  value  for  outside  turns  */ 

switch  (currentPath. Type)  { 
case  TRACKRWALLMODE : 
sonarNum  =  S090; 
deltaAngle  =  PI/2.0; 
break; 

case  TRACKLWALLMODE : 
sonarNum  =  S270; 
deltaAngle  =-PI/2.0; 
break; 
default: 

print f ("\nERROR  in  wallHugRule ()")  ; 

exit (1) ; 

break; 

} 
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67 

68  /*  offset  initially  stored  in  currentPath. Radius  */ 

69  if  (firstTimeCalled)  { 

70  offset  -  currentPath. Radius; 

71  } 

72 

73  /*  Get  current  configuation  of  the  linear  fitted  (w/  decay)  wall  */ 

74  curSeg  -  GetSegmentDecay {sonarNum) ; 

75 

76  /*  calculate  the  wall  angle  */ 

77  if  (fabs (norm( (curSeg. alpha  -  PI/2.0)  -  vehicle. Theta) )  <  PI/2.0)  { 

78  wallTheta  =  norm (curSeg. alpha  -  PI/2.0); 

79  } 

80  else  { 

81  wallTheta  *  norm  (curSeg. alpha  +  PI/2.0); 

82  } 

83 

84 

85  /*  first  check  forward  collision  eminent  */ 

86  if  (halted  | | 

87  ( (sonar_table [S000] .update)  && 

88  (sonar_table [SOOO] .d  <  COLLISIONJFHRESH) ) )  { 

89  halted  =1; 

90  commanded . Linear  «  0.0; 

91  commanded. Rotational  =  0.0; 

92  return  commanded; 

93  } 

94 

95  /*  ensure  good  side  sonar  history  data  */ 

96  if  < IpingedTwice  &&  sonar_table [sonarNum] .update)  { 

97  sonarjtable [sonarNum] .update  =  0; 

98  if  (pingedOnce)  { 

99  pingedTwice  =  1; 

100  } 

101  else  { 

102  pingedOnce  =  1; 

103  } 

104  } 

105 


106  /* - */ 

107  /*  Handle  all  phases:  wall  following,  time  to  turn,  in  turn,  */ 

108  /*  in  transition,  return  to  wall  following  from  turn  */ 

109  /* - */ 

110 


111  /*  not  in  turn  (following  wall),  check  for  turn  conditions  */ 

112  if  ( ! inTurn)  { 

113 

114  /*  set  up  sonar  distances  used  in  checks  */ 

115  distOOO  =  sonar_table [SOOO] .d  +  sonar_table [SOOO] .SonarPosit .X; 

116  sideDist  =  sonar_table [sonarNum] .d; 

117  lastSideDist  »  sonar_table [sonarNum] .d0; 

118 

119  /*  check  distance  from  front  wall,  if  past  neutral  switching  point  */ 

120  /*  start  inside  turn  to  new  path  */ 

121  if  (  (sonarjtable [SOOO] .update)  && 

122  (distOOO  <=  3 . 0*INSIDE_JTURN_SIGMA*PI/2 . 0  +  fabs (of f set) )  )  { 

123  inTurn  =  1; 

124  turnType  =  0; 

125  savedSigma  =  sigmaValue { ) ; 

126  setSigmalmm (INSIDE_TURN_SIGMA)  ; 

127  of fsetConfig  =  defineConfig (distOOO-fabs (offset) ,  0.0,  deltaAngle,  0.0) 

128 

129  /*  set  current  path  to  approximate  next  wall  offset  path  */ 

130  currentPath . conf ig  -  compose (& vehicle,  &of fsetConfig) ; 

131  cur rentPa th. conf ig. Kappa  =0.0; 

132  k  =  1. 0 /sigmaValue () ; 

133  currentPath. a  =  3.0*k  ; 

134  currentPath. b  =  3.0*k*k; 

135  currentPath. c  =  k*k*k; 

136 

137  trackType  =  1; 
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sonar_table [S000] .update  =  0; 
DisableSonar (sonarNum) ; 


/*  check  for  an  outside  turn,  indicated  by  large  difference  between  */ 

/*  current  and  previous  side  sonar  measurements  */ 

else  if  (  (sonar_table [sonarNum]  .update)  && 

(pingedTwice)  && 

(fabs (sideDist  -  lastSideDist)  >  OUTSIDEJTURNJTHRESH)  )  { 
inTurn  -  1; 

savedSigma  =  sigma  Value  ( ) ; 
setSigmalmm  ( OUTSIDE JTURN_SIGMA)  ; 
turnType  =2; 

/*  make  current  path  a  circle  around  the  corner  */ 

/*  The  radius  was  changed  from  the  offset  value  to  */ 

/*  3/4  offset  value  for  a  little  tighter  turn  */ 

initCircleTheta  =  norm (vehicle. Theta ) ;  /*  save  for  later  comparison  */ 

kk  -  -1.0/ (of fset*3. 0/4 .0) ; 

kk2  =  kk  *  kk; 

k  =  1.0/sigmaValue() ; 

currentPath . a  -  3.0*k  ; 

currentPath.b  =  3.0*k*k  -  kk2  ; 

currentPath . c  =  k*k*k  -  3.0*k*kk2  ; 

currentPath. config  =  defineConfig (vehicle. Posit. X,  vehicle . Posit . Y, 

initCircleTheta,  kk)  ; 


cur rent Path. Radius  =  1.0/kk; 

offsetConfig  =  defineConfig (0 . 0,  -offset,  0.0,  0.0); 
tempConfig  =  compose (& current Path. config,  &of fsetConfig) ; 
currentPath. Center. X  *  tempConfig.Posit.X; 
currentPath . Center . Y  =  tempConfig. Posit. Y; 

trackType  -  1; 

sonar_table [sonarNum] .update  =0; 

DisableSonar (sonarNum) ; 


f*  otherwise  keep  following  the  wall  */ 
else  { 

trackType  =  0; 

} 


/*  in  a  turn,  check  for  transition  criteria  */ 
else  if  ( linTransition)  { 

/*  check  to  see  if  within  transition  area  now,  reset  linfit  data  if  so  */ 

/*  check  for  inside  turn,  vehicle  angle  close  to  line  angle  */ 
if  (  (turnType  —  0)  && 

(fabs (norm (vehicle. Theta  -  currentPath. config. Theta) )  < 

TRANSITION  ANGLE) ) { 

mTransition  =  1;  “ 

Enable Sonar (sonarNum) ; 

Re setMoments Dec ay (sonarNum) ; 
pingedOnce  -  pingedTwice  =  0; 

} 

/*  for  outside  turn  check  if  past  the  corner  */ 
else  if  (turnType  ==  1)  { 

inverseConfig  =  inverse (currentPath. config) ; 
relativeConfig  =  compose (& inverseConfig,  svehicle); 

/*  check  if  past  the  wall  corner  */ 
if  (relativeConfig. Posit. X  >  25)  { 

inTransition  =  1; 

Enable Sonar (sonarNum) ; 

ResetMomentsDecay (sonarNum) ; 
pingedOnce  =  pingedTwice  -  0; 

} 

} 

/*  if  still  on  circle,  check  to  see  if  angle  has  changed  by  */ 
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/*  90  +/-  3  degrees  from  the  last  wall  or  the  initial  circle  angle,  */ 

/*  whichever  comes  first  */ 
else  if  {  (turnType  ==  2)  && 

( (fabs (norm (vehicle. Theta  -  prevWallTheta  +  deltaAngle))  <= 

3*d2r)  I | 

(fabs (norm (vehicle .Theta  -  initCircleTheta  +  deltaAngle))  <= 

3*d2r)  ) )  { 

/*  proceed  on  a  line  straight  ahead  */ 

currentPath.config  -  defineConfig (vehicle. Posit. X,  vehicle .Posit .Y, 

vehicle. Theta,  0.0); 

k  =  1 . 0/sigmaValue ( ) ; 
currentPath.a  -  3.0*k  ; 
currentPath.b  -  3.0*k*k; 
currentPath.c  =  k*k*k; 

turnType  =  1; 

} 


/*  keep  tracking  toward  new  path  line  */ 
trackType  =  1; 


/*  in  transition  area  */ 
else  { 

/*  check  to  see  if  new  linfit  data  is  usable,  return  to  wall  following  */ 
if  (curSeg. usable)  { 
inTransition  =  0; 
inTurn  =  0; 
trackType  =  0; 
setSigmalmm(savedSigma)  ; 


/*  otherwise  keep  tracking  toward  new  path  */ 
else  { 

trackType  -  1; 

} 


switch  (trackType)  { 

/*  follow  wall  represented  by  linear  fitted  sonar  data  */ 
case  0: 

wallx  =  curSeg. r  *  cos (curSeg. alpha) ; 
wally  =  curSeg. r  *  sin (curSeg. alpha) ; 

/*  wallTheta  defined  earlier  */ 

curWall  =  defineConfig (wallx,  wally,  wallTheta,  0.0); 


/*  Update  the  current  path  the  robot  should  be  following  */ 
offsetConfig  =  defineConfig (0 . 0,  offset,  0.0,  0.0); 
currentPath.config  =  compose (ScurWall,  &of f setConfig) ; 

/*  Get  steering  value  and  update  commanded  velocities  */ 
jL  =  steerByWall (vehicle,  curWall,  offset); 

/*  save  wall  angle  */ 

prevWallTheta=wallTheta; 

break; 

/*  use  normal  steering  function  to  track  current  path  */ 
case  Is 

jL  -  steer (vehicle,  currentPath) ; 
break; 

default: 

printf ( ” \nERR0R  in  wallHugRule ( ) !  Exiting..."); 

exit  (1) ; 

break; 
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firstTimeCalled  -  0; 


280 
281 

282  commanded. Linear  =  computeLinVelocity ( actual. Linear /Commanded. Linear) 

283 

284  deltaDistance  -  MOTION  CONTROL  CYCLE  *  commanded . Linear ; 

285 

286  kappaCommanded  *  vehicle. Kappa  +  jL  *  deltaDistance; 

287  commanded. Rotational  -  kappaCommanded  *  commanded. Linear; 

288 

289  return  commanded; 

290  } 
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